1
/*

2
 * AFCS_MODEL1_sf.c

3
 *

4
 * Code generation for model "AFCS_MODEL1_sf".

5
 *

6
 * Model version              : 1.995

7
 * Simulink Coder version : 8.10 (R2016a) 10-Feb-2016

8
 * C source code generated on : Wed Aug 11 17:17:39 2021

9
 *

10
 * Target selection: rtwsfcn.tlc

11
 * Note: GRT includes extra infrastructure and instrumentation for prototyping

12
 * Embedded hardware selection: ARM Compatible->ARM Cortex

13
 * Emulation hardware selection:

14
 *    Differs from embedded hardware (MATLAB Host)

15
 * Code generation objectives: Unspecified

16
 * Validation result: Not run

17
 */

18


19
#include <math.h>

20
#include "AFCS_MODEL1_sf.h"

21
#include "AFCS_MODEL1_sf_private.h"

22
#undef S_FUNCTION_NAME

23
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_StickGradientDeadZone_sf)

24
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_StickGradientDeadZone_sf

25
#endif

26


27
#include "StickGradientDeadZone_sf.h"

28
#undef S_FUNCTION_NAME

29
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_PositivePass_sf)

30
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_PositivePass_sf

31
#endif

32


33
#include "PositivePass_sf.h"

34
#undef S_FUNCTION_NAME

35
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_NegativePass_sf)

36
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_NegativePass_sf

37
#endif

38


39
#include "NegativePass_sf.h"

40
#undef S_FUNCTION_NAME

41
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_WashoutFilter_sf)

42
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_WashoutFilter_sf

43
#endif

44


45
#include "WashoutFilter_sf.h"

46
#undef S_FUNCTION_NAME

47
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_RateLimiter_sf)

48
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_RateLimiter_sf

49
#endif

50


51
#include "RateLimiter_sf.h"

52
#undef S_FUNCTION_NAME

53
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_Fader_sf)

54
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_Fader_sf

55
#endif

56


57
#include "Fader_sf.h"

58
#undef S_FUNCTION_NAME

59
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_Res180_sf)

60
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_Res180_sf

61
#endif

62


63
#include "Res180_sf.h"

64
#undef S_FUNCTION_NAME

65
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_IntegratorLimited_sf)

66
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_IntegratorLimited_sf

67
#endif

68


69
#include "IntegratorLimited_sf.h"

70
#undef S_FUNCTION_NAME

71
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_LeadLagFilter_sf)

72
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_LeadLagFilter_sf

73
#endif

74


75
#include "LeadLagFilter_sf.h"

76
#undef S_FUNCTION_NAME

77
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_Gain_sf)

78
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_Gain_sf

79
#endif

80


81
#include "Gain_sf.h"

82
#undef S_FUNCTION_NAME

83
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_SaturationLimiter_sf)

84
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_SaturationLimiter_sf

85
#endif

86


87
#include "SaturationLimiter_sf.h"

88
#undef S_FUNCTION_NAME

89
#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_LagFilter_sf)

90
#define RTW_GENERATED_SFCN_TUNABLE_PRMS_LagFilter_sf

91
#endif

92


93
#include "LagFilter_sf.h"

94
#undef S_FUNCTION_NAME

95
#define S_FUNCTION_NAME                AFCS_MODEL1_sf

96
#include "simstruc.h"

97
#include "fixedpoint.h"

98


99
/* Named constants for Chart: '<S21>/delayblock1' */

100
#define AFCS_MODEL1_IN_Input0          ((uint8_T)1U)

101
#define AFCS_MODEL1_IN_Input1          ((uint8_T)2U)

102
#define AFCS_MODEL1_IN_NO_ACTIVE_CHILD ((uint8_T)0U)

103
#define AFCS_MODEL1_IN_Output0         ((uint8_T)3U)

104
#define AFCS_MODEL1_IN_Output1         ((uint8_T)4U)

105
#define AFCS_MODEL1_IN_delayblock      ((uint8_T)1U)

106


107
/* Named constants for Chart: '<S299>/AirLineUpdateSucModel' */

108
#define AFCS_MODEL1_IN_Alt1            ((uint8_T)2U)

109
#define AFCS_MODEL1_IN_pre             ((uint8_T)3U)

110
#define AFCS_M_IN_AirLineUpdateSucModel ((uint8_T)1U)

111
#define AFCS__IN_AirLineUpdateSucModel1 ((uint8_T)1U)

112


113
/* Named constants for Chart: '<S24>/zeros2' */

114
#define AFCS_MODEL1_IN_zero            ((uint8_T)1U)

115


116
/* Named constants for Chart: '<S29>/LLACalibration2' */

117
#define AFCS_MODEL1_IN_Alt1_o          ((uint8_T)1U)

118
#define AFCS_MODEL1_IN_Alt2            ((uint8_T)2U)

119
#define AFCS_MODEL1_IN_AltCalibration  ((uint8_T)1U)

120


121
/* Named constants for Chart: '<S319>/Pre_Takeoff' */

122
#define AFCS_MODEL1_IN_LockState       ((uint8_T)1U)

123
#define AFCS_MODEL1_IN_Pre_Takeoff     ((uint8_T)1U)

124
#define AFCS_MODEL1_IN_StickCheck      ((uint8_T)2U)

125
#define AFCS_MODEL1_IN_TransTakeoff    ((uint8_T)3U)

126


127
/* Named constants for Chart: '<S321>/Takeoff' */

128
#define AFCS_MODEL1_IN_AbortTakeoff    ((uint8_T)1U)

129
#define AFCS_MODEL1_IN_DrControl       ((uint8_T)1U)

130
#define AFCS_MODEL1_IN_Initial         ((uint8_T)2U)

131
#define AFCS_MODEL1_IN_LiftFrontWheel  ((uint8_T)3U)

132
#define AFCS_MODEL1_IN_NormalTakeoff   ((uint8_T)2U)

133
#define AFCS_MODEL1_IN_NwsControl      ((uint8_T)4U)

134
#define AFCS_MODEL1_IN_StartLatControl ((uint8_T)5U)

135
#define AFCS_MODEL1_IN_Takeoff         ((uint8_T)1U)

136
#define AFCS_MODEL1_IN_TransClimb      ((uint8_T)6U)

137


138
/* Named constants for Chart: '<S314>/Climb2' */

139
#define AFCS_MODEL1_IN_Climb           ((uint8_T)1U)

140
#define AFCS_MODEL1_IN_PhiControl      ((uint8_T)1U)

141
#define AFCS_MODEL1_IN_PositionControl ((uint8_T)1U)

142
#define AFCS_MODEL1_IN_PositionControl1 ((uint8_T)1U)

143
#define AFCS_MODEL1_IN_TransCruise     ((uint8_T)2U)

144
#define AFCS_MODEL1_IN_YrwControl      ((uint8_T)2U)

145
#define AFCS_MODEL1_IN_YrwControl_o    ((uint8_T)1U)

146
#define AFCS_MODEL1_IN_phiLcontrol     ((uint8_T)2U)

147


148
/* Named constants for Chart: '<S315>/Cruise' */

149
#define AFCS_MODEL1_IN_AT_MODE         ((uint8_T)1U)

150
#define AFCS_MODEL1_IN_AltControl      ((uint8_T)1U)

151
#define AFCS_MODEL1_IN_Cruise          ((uint8_T)1U)

152
#define AFCS_MODEL1_IN_Hcontrol        ((uint8_T)1U)

153
#define AFCS_MODEL1_IN_HdotControl     ((uint8_T)2U)

154
#define AFCS_MODEL1_IN_PhiControl1     ((uint8_T)2U)

155
#define AFCS_MODEL1_IN_ReFly_MODE      ((uint8_T)3U)

156
#define AFCS_MODEL1_IN_YD_MODE         ((uint8_T)4U)

157
#define AFCS_MODEL1_IN_gamacontrol     ((uint8_T)2U)

158
#define AFCS_MODEL1_IN_gamacontrol1    ((uint8_T)3U)

159
#define AFCS_MODEL1_IN_thrustControl1  ((uint8_T)2U)

160
#define AFCS_MODEL_IN_GPSFailPhiControl ((uint8_T)1U)

161


162
/* Named constants for Chart: '<S313>/Approach2' */

163
#define AFCS_MODEL1_IN_Approach        ((uint8_T)1U)

164
#define AFCS_MODEL1_IN_EmYrwControl    ((uint8_T)1U)

165
#define AFCS_MODEL1_IN_FlightMode      ((uint8_T)1U)

166
#define AFCS_MODEL1_IN_LevelPosition   ((uint8_T)2U)

167
#define AFCS_MODEL1_IN_NormYrwControl  ((uint8_T)2U)

168
#define AFCS_MODEL1_IN_PhiLControl3    ((uint8_T)3U)

169
#define AFCS_MODEL1_IN_TransLand       ((uint8_T)2U)

170
#define AFCS_MODEL1_IN_TranseNorm      ((uint8_T)4U)

171
#define AFCS_MODEL1_IN_YrwControl1     ((uint8_T)2U)

172
#define AFCS_MODEL1_IN_phiLcontrol2    ((uint8_T)2U)

173


174
/* Named constants for Chart: '<S316>/Landing2' */

175
#define AFCS_MODEL1_IN_Decline         ((uint8_T)1U)

176
#define AFCS_MODEL1_IN_FlightMode_e    ((uint8_T)2U)

177
#define AFCS_MODEL1_IN_GamaControl1    ((uint8_T)2U)

178
#define AFCS_MODEL1_IN_GamaControl2    ((uint8_T)3U)

179
#define AFCS_MODEL1_IN_GoAround        ((uint8_T)4U)

180
#define AFCS_MODEL1_IN_Landing         ((uint8_T)1U)

181
#define AFCS_MODEL1_IN_LeftSpiral10s   ((uint8_T)3U)

182
#define AFCS_MODEL1_IN_PhiLontrol3     ((uint8_T)2U)

183
#define AFCS_MODEL1_IN_Pre_Land        ((uint8_T)5U)

184
#define AFCS_MODEL1_IN_TransLand_o     ((uint8_T)4U)

185
#define AFCS_MODEL1_IN_TranseNorm_c    ((uint8_T)3U)

186
#define AFCS_MODEL1_IN_VupControl3     ((uint8_T)6U)

187
#define AFCS_MODEL1_IN_YrwControl11    ((uint8_T)4U)

188


189
/* Named constants for Chart: '<S322>/Taxi' */

190
#define AFCS_MODEL1_IN_EndControl      ((uint8_T)2U)

191
#define AFCS_MODEL1_IN_NoDrControl     ((uint8_T)3U)

192
#define AFCS_MODEL1_IN_Taxi            ((uint8_T)1U)

193
#define AFCS_MODEL1_IN_ThetaControl    ((uint8_T)5U)

194


195
/* Named constants for Chart: '<S32>/wowcmp1' */

196
#define AFCS_MODEL1_IN_Enlowfg         ((uint8_T)1U)

197
#define AFCS_MODEL1_IN_count           ((uint8_T)1U)

198
#define AFCS_MODEL1_IN_enableFG        ((uint8_T)2U)

199
#define AFCS_MODEL1_IN_enableFG1       ((uint8_T)3U)

200
#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)

201


202
extern void *AFCS_MODEL1_malloc(SimStruct *S);

203


204
#endif

205


206
#ifndef __RTW_UTFREE__

207
#if defined (MATLAB_MEX_FILE)

208


209
extern void * utMalloc(size_t);

210
extern void utFree(void *);

211


212
#endif

213
#endif                                 /* #ifndef __RTW_UTFREE__ */

214


215
/* Forward declaration for local functions */

216
static real_T AFCS_MODEL1_rt_roundd_snf(real_T u);

217
static real_T AFCS_MODEL1_rt_remd_snf(real_T u0, real_T u1);

218
static void AFCS_MODEL1_sind_m(real_T *x);

219
static void AFCS_MODEL1_cosd_c(real_T *x);

220
static real_T AFCS_MODEL1_rt_atan2d_snf(real_T u0, real_T u1);

221
static real_T AFCS_MODEL1_rt_modd_snf(real_T u0, real_T u1);

222
static real32_T AFCS_MODEL1_rt_atan2f_snf(real32_T u0, real32_T u1);

223
static int32_T AFCS_MODEL1_addup_g(int32_T bA, int32_T iB, int32_T iC);

224
static void AFCS_MODEL1_GetPsi(real_T A_lon, real_T A_lat, real_T B_lon, real_T

225
  B_lat, real_T *Drange, real_T *PsiNorth);

226
static real_T AFCS_MODEL1_rt_powd_snf(real_T u0, real_T u1);

227
static int32_T AFCS_MODEL1_addup_l(int32_T bA, int32_T iB, int32_T iC);

228
static int32_T AFCS_MODEL1_addup_p(boolean_T bA, int32_T iB, int32_T iC);

229
static int32_T AFCS_MODEL1_addup_i(int32_T bA, int32_T iB, int32_T iC);

230
static int32_T AFCS_MODEL1_addup_k(int32_T bA, int32_T iB, int32_T iC);

231
static void AFCS_MODEL1_TransformLLATBDX(real_T Lon, real_T Lat, real_T Alt,

232
  real_T Lon0, real_T Lat0, real_T Alt0, real_T *pn, real_T *pe, real_T *pd);

233
static void AFCS_MODEL1_linspace(real_T y[32]);

234
static int32_T AFCS_MODEL1_addup_h(int32_T bA, int32_T iB, int32_T iC);

235
static real32_T AFCS_MODEL1_rt_roundf_snf(real32_T u);

236
static real32_T AFCS_MODEL1_rt_remf_snf(real32_T u0, real32_T u1);

237
static int32_T AFCS_MODEL1_addup_lc(boolean_T bA, int32_T iB, int32_T iC);

238
static void AFCS_MODEL1_NormalTakeoff(SimStruct *S);

239
static int32_T AFCS_MODEL1_addup_jz(boolean_T bA, int32_T iB, int32_T iC);

240
static real32_T AFCS_MODEL1_res180_m(real32_T angle);

241
static void AFCS_MODEL1_Pre_Land(SimStruct *S);

242
static void AFCS_MODE_exit_internal_Landing(SimStruct *S);

243
static void AFCS_MODEL1_XYPositionControl_d(SimStruct *S);

244
static void AFCS_MODEL1_Landing(SimStruct *S);

245
static int32_T AFCS_MODEL1_addup_od(boolean_T bA, int32_T iB, int32_T iC);

246
static int32_T AFCS_MODEL1_addup_ke(int8_T bA, int32_T iB, int32_T iC);

247
static int32_T AFCS_MODEL1_addup_j(boolean_T bA, int32_T iB, int32_T iC);

248
static int32_T AFCS_MODEL1_addup_f(boolean_T bA, int32_T iB, int32_T iC);

249
static real32_T AFCS_MODEL1_res180_l(real32_T angle);

250
static void AFCS_MODEL1_XYPositionControl(SimStruct *S);

251
static void AFCS_MODEL1_tand(real_T *x);

252
static real32_T AFCS_MODEL1_res180(real32_T angle);

253
static int32_T AFCS_MODEL1_addup_o(boolean_T bA, int32_T iB, int32_T iC);

254
static void AFCS_MODEL1_PGT_CalculateRmin(const real_T xValues[32], const real_T

255
  yValues[32], int8_T IndataLength, real_T Rmin[32], real_T b_RminNew[255]);

256
static void rt_mrdivide_U1d1x3_U2d_9vOrDY_k(const real_T u0[3], const real_T u1

257
  [9], real_T y[3]);

258
static void AFCS_MODEL1_PGT_changewaypoints(const real_T xValues[32], const

259
  real_T yValues[32], const real_T hValues[32], int8_T IndataLength, const

260
  real_T Rmin[255], const real_T vValues[32], real_T vx_qiedian_out[255], real_T

261
  vy_qiedian_out[255], real_T xn_qiedian_out[255], real_T ye_qiedian_out[255],

262
  real_T zh_qiedian_out[255]);

263
static int32_T AFCS_MODEL1_addup(int32_T bA, int32_T iB, int32_T iC);

264
static void AFCS_MODEL1_CompDistance(real32_T X1, real32_T Y1, real32_T X2,

265
  real32_T Y2, real32_T Xac, real32_T Yac, real32_T *DD, real32_T *A, real32_T

266
  *Xrw, real32_T *Yrw);

267
static int32_T AFCS_MODEL1_addup_m(int32_T bA, int32_T iB, int32_T iC);

268
static void AFCS_MODEL1_cosd(real32_T *x);

269
static void PGT_followOrbit_nonlinear_guida(real_T cn, real_T ce, real_T roup,

270
  real_T lamda, real_T pn, real_T pe, real_T vn, real_T ve, real_T psi, real_T

271
  zz, real_T nav_param, real_T *hc, real_T *psic, real_T *acmd);

272
static void AFCS_MODEL1_sind(real32_T *x);

273
static void AFCS_MODEL1_cosd_i(real_T *x);

274
static void AFCS_MODEL1_sind_k(real_T *x);

275
static void AFCS_MODEL1_PGT_circledot(real_T x, real_T y, real_T b_vx, real_T

276
  b_vy, real_T r, real_T *x_dot, real_T *y_dot, real_T *fi);

277
static void AFCS_MODEL1_PGT_Dubins_2D(real_T x1, real_T b_y1, real_T v1x, real_T

278
  v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T *xq1,

279
  real_T *yq1, real_T *fis, real_T *fi1, real_T *xq2, real_T *yq2, real_T *fif,

280
  real_T *fi2, real_T *L, real_T *A, real_T *B, real_T *C, real_T *x0_dot,

281
  real_T *y0_dot, real_T *tx_dot, real_T *ty_dot);

282
static void AFCS_MODEL1_PGT_circledot_b(real_T x, real_T y, real_T b_vx, real_T

283
  b_vy, real_T r, real_T *x_dot, real_T *y_dot, real_T *fi);

284
static void AFCS_MODEL1_PGT_Dubins_2D_d(real_T x1, real_T b_y1, real_T v1x,

285
  real_T v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T

286
  *xq1, real_T *yq1, real_T *fis, real_T *fi1, real_T *xq2, real_T *yq2, real_T *

287
  fif, real_T *fi2, real_T *L, real_T *A, real_T *B, real_T *C, real_T *x0_dot,

288
  real_T *y0_dot, real_T *tx_dot, real_T *ty_dot);

289
static void AFCS_MODEL1_PGT_Dubins_2Dmin(real_T x1, real_T b_y1, real_T v1x,

290
  real_T v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T

291
  *xq1, real_T *yq1, real_T *fis, real_T *fi1, real_T *Lamda1, real_T *xq2,

292
  real_T *yq2, real_T *fif, real_T *fi2, real_T *Lamda2, real_T *L, real_T

293
  *x0_dot, real_T *y0_dot, real_T *tx_dot, real_T *ty_dot);

294
static void AFCS_MODE_PGT_Guidance3D_Dubins(real_T xs, real_T ys, real_T hs,

295
  real_T vxs, real_T vys, real_T xe, real_T ye, real_T he, real_T vxe, real_T

296
  vye, real_T Rmin, real_T *R, real_T *Gama, real_T *xq1, real_T *yq1, real_T

297
  *hq1, real_T *fis, real_T *Lamda1, real_T *xq2, real_T *yq2, real_T *hq2,

298
  real_T *fi2, real_T *Lamda2, real_T *x0_dot, real_T *y0_dot, real_T *tx_dot,

299
  real_T *ty_dot, real_T *fi1sum, real_T *fi2sum);

300


301
#if defined(MATLAB_MEX_FILE)

302
#include "rt_nonfinite.c"

303
#endif

304


305
static const char_T *RT_MEMORY_ALLOCATION_ERROR =

306
  "memory allocation error in generated S-Function";

307


308
/* Lookup 1D UtilityAFCS_MODEL1_sf_LookUpE_4Ol9QRGv */

309
void AFCS_MODEL1_sf_LookUpE_4Ol9QRGv(real_T *pY, const real_T *pYData, real_T u,

310
  real_T valueLo, uint32_T iHi, real_T uSpacing)

311
{

312
  if (u <= valueLo ) {

313
    (*pY) = (*pYData);

314
  } else {

315
    real_T uAdjusted = u - valueLo;

316
    real_T tmpIdxLeft = uAdjusted / uSpacing;

317
    uint32_T iLeft = (uint32_T)tmpIdxLeft;

318
    if ((tmpIdxLeft >= 4294967296.0) || (iLeft >= iHi) ) {

319
      (*pY) = pYData[iHi];

320
    } else {

321
      {

322
        real_T lambda;

323


324
        {

325
          real_T num = (real_T)uAdjusted - ( iLeft * uSpacing );

326
          lambda = num / uSpacing;

327
        }

328


329
        {

330
          real_T yLeftCast;

331
          real_T yRghtCast;

332
          yLeftCast = pYData[iLeft];

333
          yRghtCast = pYData[((iLeft)+1)];

334
          yLeftCast += lambda * ( yRghtCast - yLeftCast );

335
          (*pY) = yLeftCast;

336
        }

337
      }

338
    }

339
  }

340
}

341


342
/*

343
 *     Taylor Series expansion approximation of WGS84 model of

344
 *     ellipsoid normal gravity

345
 */

346
void wgs84_taylor_series(real_T *h, real_T *phi, real_T opt_m2ft, real_T *y,

347
  int_T k)

348
{

349
  real_T gamma_ts, m, sinphi, sin2phi;

350
  int_T i;

351
  for (i = 0; i < k; i++ ) {

352
    sinphi = sin(phi[i]);

353
    sin2phi = sinphi*sinphi;

354


355
    /* Calculate theoretical normal gravity (gamma) /eq. 4-1/ */

356
    gamma_ts = (WGS84_G_E)*( 1.0 + (WGS84_K)*sin2phi )/( sqrt(1.0 - (WGS84_E_2)*

357
      sin2phi) );

358
    m = (WGS84_A)*(WGS84_A)*(WGS84_B)*(WGS84_W_DEF)*(WGS84_W_DEF)/

359
      (WGS84_GM_DEF);

360


361
    /* Return normal gravity as the output /eq. 4-3/ */

362
    y[i] = opt_m2ft*gamma_ts*( 1.0 - 2.0*( 1.0 + 1.0/(WGS84_INV_F) + m -

363
      2.0*sin2phi/(WGS84_INV_F) )*h[i]/(WGS84_A) + 3.0*h[i]*h[i]/(

364
      (WGS84_A)*(WGS84_A)) );

365
  }

366
}

367


368
real32_T AFCS_MODEL1_look1_iflf_binlcpw(real32_T u0, const real32_T bp0[], const

369
  real32_T table[], uint32_T maxIndex)

370
{

371
  real32_T frac;

372
  uint32_T iRght;

373
  uint32_T iLeft;

374
  uint32_T bpIdx;

375


376
  /* Lookup 1-D

377
     Search method: 'binary'

378
     Use previous index: 'off'

379
     Interpolation method: 'Linear'

380
     Extrapolation method: 'Clip'

381
     Use last breakpoint for index at or above upper limit: 'off'

382
     Remove protection against out-of-range input in generated code: 'off'

383
   */

384
  /* Prelookup - Index and Fraction

385
     Index Search method: 'binary'

386
     Extrapolation method: 'Clip'

387
     Use previous index: 'off'

388
     Use last breakpoint for index at or above upper limit: 'off'

389
     Remove protection against out-of-range input in generated code: 'off'

390
   */

391
  if (u0 <= bp0[0U]) {

392
    iLeft = 0U;

393
    frac = 0.0F;

394
  } else if (u0 < bp0[maxIndex]) {

395
    /* Binary Search */

396
    bpIdx = maxIndex >> 1U;

397
    iLeft = 0U;

398
    iRght = maxIndex;

399
    while (iRght - iLeft > 1U) {

400
      if (u0 < bp0[bpIdx]) {

401
        iRght = bpIdx;

402
      } else {

403
        iLeft = bpIdx;

404
      }

405


406
      bpIdx = (iRght + iLeft) >> 1U;

407
    }

408


409
    frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1U] - bp0[iLeft]);

410
  } else {

411
    iLeft = maxIndex - 1U;

412
    frac = 1.0F;

413
  }

414


415
  /* Interpolation 1-D

416
     Interpolation method: 'Linear'

417
     Use last breakpoint for index at or above upper limit: 'off'

418
     Overflow mode: 'portable wrapping'

419
   */

420
  return (table[iLeft + 1U] - table[iLeft]) * frac + table[iLeft];

421
}

422


423
real32_T AFCS_MODEL1_look1_iflf_binlxpw(real32_T u0, const real32_T bp0[], const

424
  real32_T table[], uint32_T maxIndex)

425
{

426
  real32_T frac;

427
  uint32_T iRght;

428
  uint32_T iLeft;

429
  uint32_T bpIdx;

430


431
  /* Lookup 1-D

432
     Search method: 'binary'

433
     Use previous index: 'off'

434
     Interpolation method: 'Linear'

435
     Extrapolation method: 'Linear'

436
     Use last breakpoint for index at or above upper limit: 'off'

437
     Remove protection against out-of-range input in generated code: 'off'

438
   */

439
  /* Prelookup - Index and Fraction

440
     Index Search method: 'binary'

441
     Extrapolation method: 'Linear'

442
     Use previous index: 'off'

443
     Use last breakpoint for index at or above upper limit: 'off'

444
     Remove protection against out-of-range input in generated code: 'off'

445
   */

446
  if (u0 <= bp0[0U]) {

447
    iLeft = 0U;

448
    frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]);

449
  } else if (u0 < bp0[maxIndex]) {

450
    /* Binary Search */

451
    bpIdx = maxIndex >> 1U;

452
    iLeft = 0U;

453
    iRght = maxIndex;

454
    while (iRght - iLeft > 1U) {

455
      if (u0 < bp0[bpIdx]) {

456
        iRght = bpIdx;

457
      } else {

458
        iLeft = bpIdx;

459
      }

460


461
      bpIdx = (iRght + iLeft) >> 1U;

462
    }

463


464
    frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1U] - bp0[iLeft]);

465
  } else {

466
    iLeft = maxIndex - 1U;

467
    frac = (u0 - bp0[maxIndex - 1U]) / (bp0[maxIndex] - bp0[maxIndex - 1U]);

468
  }

469


470
  /* Interpolation 1-D

471
     Interpolation method: 'Linear'

472
     Use last breakpoint for index at or above upper limit: 'off'

473
     Overflow mode: 'portable wrapping'

474
   */

475
  return (table[iLeft + 1U] - table[iLeft]) * frac + table[iLeft];

476
}

477


478
real_T AFCS_MODEL1_look1_binlxpw(real_T u0, const real_T bp0[], const real_T

479
  table[], uint32_T maxIndex)

480
{

481
  real_T frac;

482
  uint32_T iRght;

483
  uint32_T iLeft;

484
  uint32_T bpIdx;

485


486
  /* Lookup 1-D

487
     Search method: 'binary'

488
     Use previous index: 'off'

489
     Interpolation method: 'Linear'

490
     Extrapolation method: 'Linear'

491
     Use last breakpoint for index at or above upper limit: 'off'

492
     Remove protection against out-of-range input in generated code: 'off'

493
   */

494
  /* Prelookup - Index and Fraction

495
     Index Search method: 'binary'

496
     Extrapolation method: 'Linear'

497
     Use previous index: 'off'

498
     Use last breakpoint for index at or above upper limit: 'off'

499
     Remove protection against out-of-range input in generated code: 'off'

500
   */

501
  if (u0 <= bp0[0U]) {

502
    iLeft = 0U;

503
    frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]);

504
  } else if (u0 < bp0[maxIndex]) {

505
    /* Binary Search */

506
    bpIdx = maxIndex >> 1U;

507
    iLeft = 0U;

508
    iRght = maxIndex;

509
    while (iRght - iLeft > 1U) {

510
      if (u0 < bp0[bpIdx]) {

511
        iRght = bpIdx;

512
      } else {

513
        iLeft = bpIdx;

514
      }

515


516
      bpIdx = (iRght + iLeft) >> 1U;

517
    }

518


519
    frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1U] - bp0[iLeft]);

520
  } else {

521
    iLeft = maxIndex - 1U;

522
    frac = (u0 - bp0[maxIndex - 1U]) / (bp0[maxIndex] - bp0[maxIndex - 1U]);

523
  }

524


525
  /* Interpolation 1-D

526
     Interpolation method: 'Linear'

527
     Use last breakpoint for index at or above upper limit: 'off'

528
     Overflow mode: 'portable wrapping'

529
   */

530
  return (table[iLeft + 1U] - table[iLeft]) * frac + table[iLeft];

531
}

532


533
real_T AFCS_MODEL1_look2_binlxpw(real_T u0, real_T u1, const real_T bp0[], const

534
  real_T bp1[], const real_T table[], const uint32_T maxIndex[], uint32_T stride)

535
{

536
  real_T frac;

537
  uint32_T bpIndices[2];

538
  real_T fractions[2];

539
  real_T yL_1d;

540
  uint32_T iRght;

541
  uint32_T bpIdx;

542
  uint32_T iLeft;

543


544
  /* Lookup 2-D

545
     Search method: 'binary'

546
     Use previous index: 'off'

547
     Interpolation method: 'Linear'

548
     Extrapolation method: 'Linear'

549
     Use last breakpoint for index at or above upper limit: 'off'

550
     Remove protection against out-of-range input in generated code: 'off'

551
   */

552
  /* Prelookup - Index and Fraction

553
     Index Search method: 'binary'

554
     Extrapolation method: 'Linear'

555
     Use previous index: 'off'

556
     Use last breakpoint for index at or above upper limit: 'off'

557
     Remove protection against out-of-range input in generated code: 'off'

558
   */

559
  if (u0 <= bp0[0U]) {

560
    iLeft = 0U;

561
    frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]);

562
  } else if (u0 < bp0[maxIndex[0U]]) {

563
    /* Binary Search */

564
    bpIdx = maxIndex[0U] >> 1U;

565
    iLeft = 0U;

566
    iRght = maxIndex[0U];

567
    while (iRght - iLeft > 1U) {

568
      if (u0 < bp0[bpIdx]) {

569
        iRght = bpIdx;

570
      } else {

571
        iLeft = bpIdx;

572
      }

573


574
      bpIdx = (iRght + iLeft) >> 1U;

575
    }

576


577
    frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1U] - bp0[iLeft]);

578
  } else {

579
    iLeft = maxIndex[0U] - 1U;

580
    frac = (u0 - bp0[maxIndex[0U] - 1U]) / (bp0[maxIndex[0U]] - bp0[maxIndex[0U]

581
      - 1U]);

582
  }

583


584
  fractions[0U] = frac;

585
  bpIndices[0U] = iLeft;

586


587
  /* Prelookup - Index and Fraction

588
     Index Search method: 'binary'

589
     Extrapolation method: 'Linear'

590
     Use previous index: 'off'

591
     Use last breakpoint for index at or above upper limit: 'off'

592
     Remove protection against out-of-range input in generated code: 'off'

593
   */

594
  if (u1 <= bp1[0U]) {

595
    iLeft = 0U;

596
    frac = (u1 - bp1[0U]) / (bp1[1U] - bp1[0U]);

597
  } else if (u1 < bp1[maxIndex[1U]]) {

598
    /* Binary Search */

599
    bpIdx = maxIndex[1U] >> 1U;

600
    iLeft = 0U;

601
    iRght = maxIndex[1U];

602
    while (iRght - iLeft > 1U) {

603
      if (u1 < bp1[bpIdx]) {

604
        iRght = bpIdx;

605
      } else {

606
        iLeft = bpIdx;

607
      }

608


609
      bpIdx = (iRght + iLeft) >> 1U;

610
    }

611


612
    frac = (u1 - bp1[iLeft]) / (bp1[iLeft + 1U] - bp1[iLeft]);

613
  } else {

614
    iLeft = maxIndex[1U] - 1U;

615
    frac = (u1 - bp1[maxIndex[1U] - 1U]) / (bp1[maxIndex[1U]] - bp1[maxIndex[1U]

616
      - 1U]);

617
  }

618


619
  /* Interpolation 2-D

620
     Interpolation method: 'Linear'

621
     Use last breakpoint for index at or above upper limit: 'off'

622
     Overflow mode: 'portable wrapping'

623
   */

624
  bpIdx = iLeft * stride + bpIndices[0U];

625
  yL_1d = (table[bpIdx + 1U] - table[bpIdx]) * fractions[0U] + table[bpIdx];

626
  bpIdx += stride;

627
  return (((table[bpIdx + 1U] - table[bpIdx]) * fractions[0U] + table[bpIdx]) -

628
          yL_1d) * frac + yL_1d;

629
}

630


631
/* System initialize for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */

632
void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Init(SimStruct * const S,

633
  B_LMainGearGeometryForcesandM_T *localB)

634
{

635
  /* SystemInitialize for Enabled SubSystem: '<S618>/GenerateGroundFM' */

636
  /* InitializeConditions for Memory: '<S660>/Xcmpr_prev' */

637
  ((real_T *)ssGetDWork(S, 629))[0] = 0.0;

638


639
  /* SystemInitialize for Outport: '<S651>/FM' */

640
  localB->LocalForcestoBodyAxes1[0] = 0.0;

641
  localB->Gain2[0] = 0.0;

642
  localB->LocalForcestoBodyAxes1[1] = 0.0;

643
  localB->Gain2[1] = 0.0;

644
  localB->LocalForcestoBodyAxes1[2] = 0.0;

645
  localB->Gain2[2] = 0.0;

646


647
  /* SystemInitialize for Outport: '<S651>/GearCmprs' */

648
  localB->RangeLimit = 0.0;

649


650
  /* End of SystemInitialize for SubSystem: '<S618>/GenerateGroundFM' */

651
}

652


653
/* Disable for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */

654
void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Disable(SimStruct * const S,

655
  B_LMainGearGeometryForcesandM_T *localB)

656
{

657
  /* Disable for Enabled SubSystem: '<S618>/GenerateGroundFM' */

658
  if (((boolean_T *)ssGetDWork(S, 640))[0]) {

659
    /* Disable for Outport: '<S651>/FM' */

660
    localB->LocalForcestoBodyAxes1[0] = 0.0;

661
    localB->Gain2[0] = 0.0;

662
    localB->LocalForcestoBodyAxes1[1] = 0.0;

663
    localB->Gain2[1] = 0.0;

664
    localB->LocalForcestoBodyAxes1[2] = 0.0;

665
    localB->Gain2[2] = 0.0;

666


667
    /* Disable for Outport: '<S651>/GearCmprs' */

668
    localB->RangeLimit = 0.0;

669
    ((boolean_T *)ssGetDWork(S, 640))[0] = false;

670
  }

671


672
  /* End of Disable for SubSystem: '<S618>/GenerateGroundFM' */

673
}

674


675
/* Start for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */

676
void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Start(SimStruct * const S)

677
{

678
  /* Start for Enabled SubSystem: '<S618>/GenerateGroundFM' */

679
  ((boolean_T *)ssGetDWork(S, 640))[0] = false;

680
  ssSetSolverNeedsReset(S);

681
}

682


683
/* Outputs for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */

684
void AFCS_MODEL1_LMainGearGeometryForcesandMoments(SimStruct * const S, real_T

685
  rtu_ground_z_m, real_T rtu_plant, real_T rtu_plant_d, real_T rtu_plant_f,

686
  const real_T rtu_GearLoc[3], real_T rtu_FricCoeff, real_T rtu_FricCoeff_g,

687
  B_LMainGearGeometryForcesandM_T *localB, ConstB_LMainGearGeometryForce_T

688
  *localC)

689
{

690
  real_T cosOut;

691
  int32_T i;

692
  if (ssIsSampleHit(S, 1, 0)) {

693
  }

694


695
  if (1) {

696
    /* Sum: '<S618>/Subtract' */

697
    localB->Gain = rtu_plant + rtu_ground_z_m;

698


699
    /* Switch: '<S618>/Switch1' */

700
    localB->Switch1 = (int8_T)!(localB->Gain > 15.0);

701
  }

702


703
  if (ssIsSampleHit(S, 1, 0)) {

704
    /* SignalConversion: '<S618>/HiddenBuf_InsertedFor_GenerateGroundFM_at_inport_5' */

705
    localB->HiddenBuf_InsertedFor_GenerateG = localB->Switch1;

706


707
    /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:

708
     *  EnablePort: '<S651>/Enable'

709
     */

710
    if (ssIsMajorTimeStep(S)) {

711
      if (localB->HiddenBuf_InsertedFor_GenerateG > 0) {

712
        if (!((boolean_T *)ssGetDWork(S, 640))[0]) {

713
          if (ssGetTaskTime(S,1) != ssGetTStart(S)) {

714
            ssSetSolverNeedsReset(S);

715
          }

716


717
          ((boolean_T *)ssGetDWork(S, 640))[0] = true;

718
        }

719
      } else {

720
        if (((boolean_T *)ssGetDWork(S, 640))[0]) {

721
          ssSetSolverNeedsReset(S);

722


723
          /* Disable for Outport: '<S651>/FM' */

724
          localB->LocalForcestoBodyAxes1[0] = 0.0;

725
          localB->Gain2[0] = 0.0;

726
          localB->LocalForcestoBodyAxes1[1] = 0.0;

727
          localB->Gain2[1] = 0.0;

728
          localB->LocalForcestoBodyAxes1[2] = 0.0;

729
          localB->Gain2[2] = 0.0;

730


731
          /* Disable for Outport: '<S651>/GearCmprs' */

732
          localB->RangeLimit = 0.0;

733
          ((boolean_T *)ssGetDWork(S, 640))[0] = false;

734
        }

735
      }

736
    }

737


738
    /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */

739
  }

740


741
  /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:

742
   *  EnablePort: '<S651>/Enable'

743
   */

744
  if (((boolean_T *)ssGetDWork(S, 640))[0]) {

745
    if (ssIsSampleHit(S, 1, 0)) {

746
      /* Memory: '<S660>/Xcmpr_prev' */

747
      localB->Xcmpr_prev = ((real_T *)ssGetDWork(S, 629))[0];

748
    }

749


750
    if (1) {

751
      /* SignalConversion: '<S656>/TmpSignal ConversionAtSinCosInport1' incorporates:

752
       *  Constant: '<S651>/Constant'

753
       */

754
      localB->SinCos_o1_e[0] = rtu_plant_d;

755
      localB->SinCos_o1_e[1] = rtu_plant_f;

756
      localB->SinCos_o1_e[2] = 0.0;

757


758
      /* Trigonometry: '<S656>/SinCos' */

759
      cosOut = cos(localB->SinCos_o1_e[0]);

760
      localB->SinCos_o1_e[0] = sin(localB->SinCos_o1_e[0]);

761
      localB->TmpSignalConversionAtProduct3In[0] = cosOut;

762
      cosOut = cos(localB->SinCos_o1_e[1]);

763
      localB->SinCos_o1_e[1] = sin(localB->SinCos_o1_e[1]);

764
      localB->TmpSignalConversionAtProduct3In[1] = cosOut;

765
      cosOut = cos(localB->SinCos_o1_e[2]);

766
      localB->SinCos_o1_e[2] = sin(localB->SinCos_o1_e[2]);

767
      localB->TmpSignalConversionAtProduct3In[2] = cosOut;

768


769
      /* Product: '<S661>/u(5)*u(6)' */

770
      localB->VectorConcatenate_n[0] = localB->TmpSignalConversionAtProduct3In[1]

771
        * localB->TmpSignalConversionAtProduct3In[2];

772


773
      /* Product: '<S664>/u(6)*u(1)*u(2)' */

774
      localB->kxi = localB->TmpSignalConversionAtProduct3In[2] *

775
        localB->SinCos_o1_e[0] * localB->SinCos_o1_e[1];

776


777
      /* Product: '<S664>/u(3)*u(4)' */

778
      localB->compression = localB->SinCos_o1_e[2] *

779
        localB->TmpSignalConversionAtProduct3In[0];

780


781
      /* Sum: '<S664>/Sum' */

782
      localB->VectorConcatenate_n[1] = localB->kxi - localB->compression;

783


784
      /* Product: '<S667>/u(1)*u(3)' */

785
      localB->compression = localB->SinCos_o1_e[0] * localB->SinCos_o1_e[2];

786


787
      /* Product: '<S667>/u(2)*u(4)*u(6)' */

788
      localB->kxi = localB->SinCos_o1_e[1] *

789
        localB->TmpSignalConversionAtProduct3In[0] *

790
        localB->TmpSignalConversionAtProduct3In[2];

791


792
      /* Sum: '<S667>/Sum' */

793
      localB->VectorConcatenate_n[2] = localB->compression + localB->kxi;

794


795
      /* Product: '<S662>/u(3)*u(5)' */

796
      localB->VectorConcatenate_n[3] = localB->SinCos_o1_e[2] *

797
        localB->TmpSignalConversionAtProduct3In[1];

798


799
      /* Product: '<S665>/u(1)*u(2)*u(3)' */

800
      localB->compression = localB->SinCos_o1_e[0] * localB->SinCos_o1_e[1] *

801
        localB->SinCos_o1_e[2];

802


803
      /* Product: '<S665>/u(4)*u(6)' */

804
      localB->kxi = localB->TmpSignalConversionAtProduct3In[0] *

805
        localB->TmpSignalConversionAtProduct3In[2];

806


807
      /* Sum: '<S665>/Sum' */

808
      localB->VectorConcatenate_n[4] = localB->compression + localB->kxi;

809


810
      /* Product: '<S668>/u(2)*u(3)*u(4)' */

811
      localB->compression = localB->SinCos_o1_e[1] * localB->SinCos_o1_e[2] *

812
        localB->TmpSignalConversionAtProduct3In[0];

813


814
      /* Product: '<S668>/u(1)*u(6)' */

815
      localB->kxi = localB->TmpSignalConversionAtProduct3In[2] *

816
        localB->SinCos_o1_e[0];

817


818
      /* Sum: '<S668>/Sum' */

819
      localB->VectorConcatenate_n[5] = localB->compression - localB->kxi;

820


821
      /* Gain: '<S663>/Gain2' */

822
      localB->VectorConcatenate_n[6] = -localB->SinCos_o1_e[1];

823


824
      /* Product: '<S666>/u(1)*u(3)' */

825
      localB->VectorConcatenate_n[7] = localB->SinCos_o1_e[0] *

826
        localB->TmpSignalConversionAtProduct3In[1];

827


828
      /* Product: '<S669>/u(4)*u(5)' */

829
      localB->VectorConcatenate_n[8] = localB->TmpSignalConversionAtProduct3In[0]

830
        * localB->TmpSignalConversionAtProduct3In[1];

831


832
      /* Selector: '<S654>/get_row3' */

833
      localB->TmpSignalConversionAtProduct3In[0] = localB->VectorConcatenate_n[2];

834
      localB->TmpSignalConversionAtProduct3In[1] = localB->VectorConcatenate_n[5];

835
      localB->TmpSignalConversionAtProduct3In[2] = localB->VectorConcatenate_n[8];

836


837
      /* Product: '<S654>/prod1' */

838
      localB->compression = rtu_GearLoc[0] *

839
        localB->TmpSignalConversionAtProduct3In[0];

840


841
      /* Product: '<S654>/prod2' */

842
      localB->kxi = rtu_GearLoc[1] * localB->TmpSignalConversionAtProduct3In[1];

843


844
      /* Sum: '<S654>/Sum1' */

845
      localB->Sum1 = ((rtu_ground_z_m + rtu_plant) - localB->compression) -

846
        localB->kxi;

847


848
      /* Product: '<S660>/compression' */

849
      localB->compression = localB->Sum1 /

850
        localB->TmpSignalConversionAtProduct3In[2];

851


852
      /* Sum: '<S660>/Sum1' */

853
      localB->kxi = rtu_GearLoc[2] - localB->compression;

854


855
      /* Saturate: '<S660>/RangeLimit' */

856
      if (localB->kxi > 0.5) {

857
        localB->RangeLimit = 0.5;

858
      } else if (localB->kxi < 0.0) {

859
        localB->RangeLimit = 0.0;

860
      } else {

861
        localB->RangeLimit = localB->kxi;

862
      }

863


864
      /* End of Saturate: '<S660>/RangeLimit' */

865


866
      /* Gain: '<S660>/Gain1' */

867
      localB->kxi = 10000.0 * localB->RangeLimit;

868


869
      /* Sum: '<S660>/dCompression' */

870
      localB->jxk = localB->RangeLimit - localB->Xcmpr_prev;

871


872
      /* Gain: '<S660>/ComprRate' */

873
      localB->jxk *= 100.0;

874


875
      /* Gain: '<S660>/Gain' */

876
      localB->jxk *= 10000.0;

877


878
      /* Sum: '<S660>/Sum2' */

879
      localB->kxi += localB->jxk;

880


881
      /* Product: '<S660>/NormalForce' */

882
      localB->kxi /= localB->TmpSignalConversionAtProduct3In[2];

883


884
      /* Saturate: '<S660>/ground only pushes' */

885
      if (localB->kxi <= 0.0) {

886
        localB->kxi = 0.0;

887
      }

888


889
      /* End of Saturate: '<S660>/ground only pushes' */

890


891
      /* Gain: '<S660>/reaction' */

892
      localB->reaction = -localB->kxi;

893


894
      /* Product: '<S651>/Product8' */

895
      localB->GearLocalForcevector[0] = localB->reaction * rtu_FricCoeff;

896


897
      /* Product: '<S651>/Product9' */

898
      localB->GearLocalForcevector[1] = localB->reaction * rtu_FricCoeff_g;

899


900
      /* SignalConversion: '<S651>/ConcatBufferAtGear  Local Force vectorIn2' */

901
      localB->GearLocalForcevector[2] = localB->reaction;

902


903
      /* Product: '<S651>/Local Forces to Body Axes1' */

904
      for (i = 0; i < 3; i++) {

905
        localB->LocalForcestoBodyAxes1[i] = 0.0;

906
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_n[i] *

907
          localB->GearLocalForcevector[0];

908
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_n[i + 3] *

909
          localB->GearLocalForcevector[1];

910
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_n[i + 6] *

911
          localB->GearLocalForcevector[2];

912
      }

913


914
      /* End of Product: '<S651>/Local Forces to Body Axes1' */

915


916
      /* Product: '<S658>/j x k' */

917
      localB->jxk = rtu_GearLoc[1] * localB->LocalForcestoBodyAxes1[2];

918


919
      /* Product: '<S658>/k x i' */

920
      localB->kxi = rtu_GearLoc[2] * localB->LocalForcestoBodyAxes1[0];

921


922
      /* Product: '<S658>/i x j' */

923
      localB->ixj = rtu_GearLoc[0] * localB->LocalForcestoBodyAxes1[1];

924


925
      /* Product: '<S659>/k x j' */

926
      localB->kxj = rtu_GearLoc[2] * localB->LocalForcestoBodyAxes1[1];

927


928
      /* Product: '<S659>/i x k' */

929
      localB->ixk = rtu_GearLoc[0] * localB->LocalForcestoBodyAxes1[2];

930


931
      /* Product: '<S659>/j x i' */

932
      localB->jxi = rtu_GearLoc[1] * localB->LocalForcestoBodyAxes1[0];

933


934
      /* Sum: '<S653>/Sum' */

935
      localB->Sum[0] = localB->jxk - localB->kxj;

936
      localB->Sum[1] = localB->kxi - localB->ixk;

937
      localB->Sum[2] = localB->ixj - localB->jxi;

938
    }

939


940
    if (ssIsSampleHit(S, 1, 0)) {

941
      /* Constant: '<S657>/zero1' */

942
      localB->VectorConcatenate_b[0] = 0.0;

943
    }

944


945
    if (1) {

946
      /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn2' */

947
      localB->VectorConcatenate_b[1] = localB->compression;

948


949
      /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn3' */

950
      localB->VectorConcatenate_b[2] = localC->inv3;

951


952
      /* Gain: '<S657>/inv1' */

953
      localB->VectorConcatenate_b[3] = -localB->compression;

954
    }

955


956
    if (ssIsSampleHit(S, 1, 0)) {

957
      /* Constant: '<S657>/zero2' */

958
      localB->VectorConcatenate_b[4] = 0.0;

959
    }

960


961
    if (1) {

962
      /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn6' */

963
      localB->VectorConcatenate_b[5] = rtu_GearLoc[0];

964


965
      /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn7' */

966
      localB->VectorConcatenate_b[6] = rtu_GearLoc[1];

967


968
      /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn8' */

969
      localB->VectorConcatenate_b[7] = localC->inv2;

970
    }

971


972
    if (ssIsSampleHit(S, 1, 0)) {

973
      /* Constant: '<S657>/zero3' */

974
      localB->VectorConcatenate_b[8] = 0.0;

975
    }

976


977
    if (1) {

978
      for (i = 0; i < 3; i++) {

979
        /* Product: '<S655>/force to moment' */

980
        localB->TmpSignalConversionAtProduct3In[i] = 0.0;

981
        localB->TmpSignalConversionAtProduct3In[i] +=

982
          localB->VectorConcatenate_b[3 * i] * localB->LocalForcestoBodyAxes1[0];

983
        localB->TmpSignalConversionAtProduct3In[i] +=

984
          localB->VectorConcatenate_b[3 * i + 1] *

985
          localB->LocalForcestoBodyAxes1[1];

986
        localB->TmpSignalConversionAtProduct3In[i] +=

987
          localB->VectorConcatenate_b[3 * i + 2] *

988
          localB->LocalForcestoBodyAxes1[2];

989


990
        /* Gain: '<S651>/Gain2' */

991
        localB->Gain2[i] = AFCS_MODEL1_ConstP.pooled20[i] *

992
          localB->TmpSignalConversionAtProduct3In[i];

993
      }

994
    }

995


996
    if (ssIsSampleHit(S, 1, 0)) {

997
    }

998


999
    if (1) {

1000
      /* Math: '<S654>/Math Function1' */

1001
      for (i = 0; i < 3; i++) {

1002
        localB->MathFunction1[3 * i] = localB->VectorConcatenate_n[i];

1003
        localB->MathFunction1[1 + 3 * i] = localB->VectorConcatenate_n[i + 3];

1004
        localB->MathFunction1[2 + 3 * i] = localB->VectorConcatenate_n[i + 6];

1005
      }

1006


1007
      /* End of Math: '<S654>/Math Function1' */

1008


1009
      /* SignalConversion: '<S654>/TmpSignal ConversionAtProduct3Inport2' incorporates:

1010
       *  Constant: '<S654>/Constant1'

1011
       */

1012
      localB->TmpSignalConversionAtProduct3In[0] = rtu_GearLoc[0];

1013
      localB->TmpSignalConversionAtProduct3In[1] = rtu_GearLoc[1];

1014
      localB->TmpSignalConversionAtProduct3In[2] = 0.0;

1015


1016
      /* Product: '<S654>/Product3' */

1017
      for (i = 0; i < 3; i++) {

1018
        localB->Product3[i] = 0.0;

1019
        localB->Product3[i] += localB->MathFunction1[i] *

1020
          localB->TmpSignalConversionAtProduct3In[0];

1021
        localB->Product3[i] += localB->MathFunction1[i + 3] *

1022
          localB->TmpSignalConversionAtProduct3In[1];

1023
        localB->Product3[i] += localB->MathFunction1[i + 6] *

1024
          localB->TmpSignalConversionAtProduct3In[2];

1025
      }

1026


1027
      /* End of Product: '<S654>/Product3' */

1028


1029
      /* Sum: '<S654>/Sum5' */

1030
      localB->Sum5 = rtu_plant - localB->Product3[2];

1031
    }

1032


1033
    if (ssIsSampleHit(S, 1, 0)) {

1034
    }

1035
  }

1036


1037
  /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */

1038
  if (ssIsSampleHit(S, 1, 0)) {

1039
  }

1040


1041
  if (1) {

1042
    /* Gain: '<S618>/Gain' */

1043
    localB->Gain = -localB->LocalForcestoBodyAxes1[2];

1044


1045
    /* SignalConversion: '<S652>/TmpSignal ConversionAtSinCosInport1' incorporates:

1046
     *  Constant: '<S618>/Constant'

1047
     */

1048
    localB->SinCos_o1[0] = rtu_plant_d;

1049
    localB->SinCos_o1[1] = rtu_plant_f;

1050
    localB->SinCos_o1[2] = 0.0;

1051


1052
    /* Trigonometry: '<S652>/SinCos' */

1053
    cosOut = cos(localB->SinCos_o1[0]);

1054
    localB->SinCos_o1[0] = sin(localB->SinCos_o1[0]);

1055
    localB->SinCos_o2[0] = cosOut;

1056
    cosOut = cos(localB->SinCos_o1[1]);

1057
    localB->SinCos_o1[1] = sin(localB->SinCos_o1[1]);

1058
    localB->SinCos_o2[1] = cosOut;

1059
    cosOut = cos(localB->SinCos_o1[2]);

1060
    localB->SinCos_o1[2] = sin(localB->SinCos_o1[2]);

1061
    localB->SinCos_o2[2] = cosOut;

1062


1063
    /* Product: '<S672>/u(5)*u(6)' */

1064
    localB->VectorConcatenate[0] = localB->SinCos_o2[1] * localB->SinCos_o2[2];

1065


1066
    /* Product: '<S673>/u(3)*u(5)' */

1067
    localB->VectorConcatenate[3] = localB->SinCos_o1[2] * localB->SinCos_o2[1];

1068


1069
    /* Gain: '<S674>/Gain2' */

1070
    localB->VectorConcatenate[6] = -localB->SinCos_o1[1];

1071


1072
    /* Product: '<S675>/u(6)*u(1)*u(2)' */

1073
    localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0] *

1074
      localB->SinCos_o1[1];

1075


1076
    /* Product: '<S675>/u(3)*u(4)' */

1077
    localB->u2u3u4 = localB->SinCos_o1[2] * localB->SinCos_o2[0];

1078


1079
    /* Sum: '<S675>/Sum' */

1080
    localB->VectorConcatenate[1] = localB->u1u6 - localB->u2u3u4;

1081


1082
    /* Product: '<S676>/u(1)*u(2)*u(3)' */

1083
    localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[1] *

1084
      localB->SinCos_o1[2];

1085


1086
    /* Product: '<S676>/u(4)*u(6)' */

1087
    localB->u1u6 = localB->SinCos_o2[0] * localB->SinCos_o2[2];

1088


1089
    /* Sum: '<S676>/Sum' */

1090
    localB->VectorConcatenate[4] = localB->u2u3u4 + localB->u1u6;

1091


1092
    /* Product: '<S677>/u(1)*u(3)' */

1093
    localB->VectorConcatenate[7] = localB->SinCos_o1[0] * localB->SinCos_o2[1];

1094


1095
    /* Product: '<S678>/u(1)*u(3)' */

1096
    localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[2];

1097


1098
    /* Product: '<S678>/u(2)*u(4)*u(6)' */

1099
    localB->u1u6 = localB->SinCos_o1[1] * localB->SinCos_o2[0] *

1100
      localB->SinCos_o2[2];

1101


1102
    /* Sum: '<S678>/Sum' */

1103
    localB->VectorConcatenate[2] = localB->u2u3u4 + localB->u1u6;

1104


1105
    /* Product: '<S679>/u(2)*u(3)*u(4)' */

1106
    localB->u2u3u4 = localB->SinCos_o1[1] * localB->SinCos_o1[2] *

1107
      localB->SinCos_o2[0];

1108


1109
    /* Product: '<S679>/u(1)*u(6)' */

1110
    localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0];

1111


1112
    /* Sum: '<S679>/Sum' */

1113
    localB->VectorConcatenate[5] = localB->u2u3u4 - localB->u1u6;

1114


1115
    /* Product: '<S680>/u(4)*u(5)' */

1116
    localB->VectorConcatenate[8] = localB->SinCos_o2[0] * localB->SinCos_o2[1];

1117


1118
    /* Switch: '<S618>/Switch' */

1119
    localB->Switch = (int8_T)(localB->Gain > 3.0);

1120
  }

1121
}

1122


1123
/* Update for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */

1124
void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Update(SimStruct * const S,

1125
  B_LMainGearGeometryForcesandM_T *localB)

1126
{

1127
  /* Update for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:

1128
   *  Update for EnablePort: '<S651>/Enable'

1129
   */

1130
  if (((boolean_T *)ssGetDWork(S, 640))[0] && ssIsSampleHit(S, 1, 0)) {

1131
    /* Update for Memory: '<S660>/Xcmpr_prev' */

1132
    ((real_T *)ssGetDWork(S, 629))[0] = localB->RangeLimit;

1133
  }

1134


1135
  /* End of Update for SubSystem: '<S618>/GenerateGroundFM' */

1136
}

1137


1138
/* Termination for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */

1139
void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Term(SimStruct *const S)

1140
{

1141
}

1142


1143
/* System initialize for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */

1144
void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Init(SimStruct * const S,

1145
  B_NoseGearGeometryForcesandMo_T *localB)

1146
{

1147
  /* SystemInitialize for Enabled SubSystem: '<S620>/GenerateGroundFM' */

1148
  /* InitializeConditions for Memory: '<S693>/Xcmpr_prev' */

1149
  ((real_T *)ssGetDWork(S, 624))[0] = 0.0;

1150


1151
  /* SystemInitialize for Outport: '<S686>/FM' */

1152
  localB->LocalForcestoBodyAxes1[0] = 0.0;

1153
  localB->Gain1[0] = 0.0;

1154
  localB->LocalForcestoBodyAxes1[1] = 0.0;

1155
  localB->Gain1[1] = 0.0;

1156
  localB->LocalForcestoBodyAxes1[2] = 0.0;

1157
  localB->Gain1[2] = 0.0;

1158


1159
  /* SystemInitialize for Outport: '<S686>/GearCmprs' */

1160
  localB->RangeLimit = 0.0;

1161


1162
  /* End of SystemInitialize for SubSystem: '<S620>/GenerateGroundFM' */

1163
}

1164


1165
/* Disable for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */

1166
void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Disable(SimStruct * const S,

1167
  B_NoseGearGeometryForcesandMo_T *localB)

1168
{

1169
  /* Disable for Enabled SubSystem: '<S620>/GenerateGroundFM' */

1170
  if (((boolean_T *)ssGetDWork(S, 628))[0]) {

1171
    /* Disable for Outport: '<S686>/FM' */

1172
    localB->LocalForcestoBodyAxes1[0] = 0.0;

1173
    localB->Gain1[0] = 0.0;

1174
    localB->LocalForcestoBodyAxes1[1] = 0.0;

1175
    localB->Gain1[1] = 0.0;

1176
    localB->LocalForcestoBodyAxes1[2] = 0.0;

1177
    localB->Gain1[2] = 0.0;

1178


1179
    /* Disable for Outport: '<S686>/GearCmprs' */

1180
    localB->RangeLimit = 0.0;

1181
    ((boolean_T *)ssGetDWork(S, 628))[0] = false;

1182
  }

1183


1184
  /* End of Disable for SubSystem: '<S620>/GenerateGroundFM' */

1185
}

1186


1187
/* Start for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */

1188
void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Start(SimStruct * const S)

1189
{

1190
  /* Start for Enabled SubSystem: '<S620>/GenerateGroundFM' */

1191
  ((boolean_T *)ssGetDWork(S, 628))[0] = false;

1192
  ssSetSolverNeedsReset(S);

1193
}

1194


1195
/* Outputs for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */

1196
void AFCS_MODEL1_NoseGearGeometryForcesandMoments(SimStruct * const S, real_T

1197
  rtu_ground_z_m, real_T rtu_plant, real_T rtu_plant_a, real_T rtu_plant_j,

1198
  const real_T rtu_GearLoc[3], real_T rtu_FricCoeff, real_T rtu_FricCoeff_j,

1199
  const real_T rtu_Euler[3], B_NoseGearGeometryForcesandMo_T *localB,

1200
  ConstB_NoseGearGeometryForces_T *localC)

1201
{

1202
  real_T cosOut;

1203
  int32_T i;

1204
  if (1) {

1205
    /* Sum: '<S620>/Subtract' */

1206
    localB->Gain = rtu_plant + rtu_ground_z_m;

1207


1208
    /* Switch: '<S620>/Switch1' */

1209
    localB->Switch1 = (int8_T)!(localB->Gain > 15.0);

1210
  }

1211


1212
  if (ssIsSampleHit(S, 1, 0)) {

1213
    /* SignalConversion: '<S620>/HiddenBuf_InsertedFor_GenerateGroundFM_at_inport_5' */

1214
    localB->HiddenBuf_InsertedFor_GenerateG = localB->Switch1;

1215


1216
    /* Outputs for Enabled SubSystem: '<S620>/GenerateGroundFM' incorporates:

1217
     *  EnablePort: '<S686>/Enable'

1218
     */

1219
    if (ssIsMajorTimeStep(S)) {

1220
      if (localB->HiddenBuf_InsertedFor_GenerateG > 0) {

1221
        if (!((boolean_T *)ssGetDWork(S, 628))[0]) {

1222
          /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:

1223
           *  EnablePort: '<S651>/Enable'

1224
           */

1225
          if (ssGetTaskTime(S,1) != ssGetTStart(S)) {

1226
            ssSetSolverNeedsReset(S);

1227
          }

1228


1229
          /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */

1230
          ((boolean_T *)ssGetDWork(S, 628))[0] = true;

1231
        }

1232
      } else {

1233
        if (((boolean_T *)ssGetDWork(S, 628))[0]) {

1234
          ssSetSolverNeedsReset(S);

1235


1236
          /* Disable for Outport: '<S686>/FM' */

1237
          localB->LocalForcestoBodyAxes1[0] = 0.0;

1238
          localB->Gain1[0] = 0.0;

1239
          localB->LocalForcestoBodyAxes1[1] = 0.0;

1240
          localB->Gain1[1] = 0.0;

1241
          localB->LocalForcestoBodyAxes1[2] = 0.0;

1242
          localB->Gain1[2] = 0.0;

1243


1244
          /* Disable for Outport: '<S686>/GearCmprs' */

1245
          localB->RangeLimit = 0.0;

1246
          ((boolean_T *)ssGetDWork(S, 628))[0] = false;

1247
        }

1248
      }

1249
    }

1250


1251
    /* End of Outputs for SubSystem: '<S620>/GenerateGroundFM' */

1252
  }

1253


1254
  /* Outputs for Enabled SubSystem: '<S620>/GenerateGroundFM' incorporates:

1255
   *  EnablePort: '<S686>/Enable'

1256
   */

1257
  if (((boolean_T *)ssGetDWork(S, 628))[0]) {

1258
    if (ssIsSampleHit(S, 1, 0)) {

1259
      /* Memory: '<S693>/Xcmpr_prev' */

1260
      localB->Xcmpr_prev = ((real_T *)ssGetDWork(S, 624))[0];

1261
    }

1262


1263
    if (1) {

1264
      /* SignalConversion: '<S690>/TmpSignal ConversionAtSinCosInport1' incorporates:

1265
       *  Constant: '<S686>/Constant'

1266
       */

1267
      localB->Product3[0] = rtu_plant_a;

1268
      localB->Product3[1] = rtu_plant_j;

1269
      localB->Product3[2] = 0.0;

1270


1271
      /* Trigonometry: '<S690>/SinCos' */

1272
      cosOut = cos(localB->Product3[0]);

1273
      localB->Product3[0] = sin(localB->Product3[0]);

1274
      localB->sincos_o2[0] = cosOut;

1275
      cosOut = cos(localB->Product3[1]);

1276
      localB->Product3[1] = sin(localB->Product3[1]);

1277
      localB->sincos_o2[1] = cosOut;

1278
      cosOut = cos(localB->Product3[2]);

1279
      localB->Product3[2] = sin(localB->Product3[2]);

1280
      localB->sincos_o2[2] = cosOut;

1281


1282
      /* Product: '<S694>/u(5)*u(6)' */

1283
      localB->VectorConcatenate_c[0] = localB->sincos_o2[1] * localB->sincos_o2

1284
        [2];

1285


1286
      /* Product: '<S697>/u(6)*u(1)*u(2)' */

1287
      localB->prod2 = localB->sincos_o2[2] * localB->Product3[0] *

1288
        localB->Product3[1];

1289


1290
      /* Product: '<S697>/u(3)*u(4)' */

1291
      localB->compression = localB->Product3[2] * localB->sincos_o2[0];

1292


1293
      /* Sum: '<S697>/Sum' */

1294
      localB->VectorConcatenate_c[1] = localB->prod2 - localB->compression;

1295


1296
      /* Product: '<S700>/u(1)*u(3)' */

1297
      localB->compression = localB->Product3[0] * localB->Product3[2];

1298


1299
      /* Product: '<S700>/u(2)*u(4)*u(6)' */

1300
      localB->prod2 = localB->Product3[1] * localB->sincos_o2[0] *

1301
        localB->sincos_o2[2];

1302


1303
      /* Sum: '<S700>/Sum' */

1304
      localB->VectorConcatenate_c[2] = localB->compression + localB->prod2;

1305


1306
      /* Product: '<S695>/u(3)*u(5)' */

1307
      localB->VectorConcatenate_c[3] = localB->Product3[2] * localB->sincos_o2[1];

1308


1309
      /* Product: '<S698>/u(1)*u(2)*u(3)' */

1310
      localB->compression = localB->Product3[0] * localB->Product3[1] *

1311
        localB->Product3[2];

1312


1313
      /* Product: '<S698>/u(4)*u(6)' */

1314
      localB->prod2 = localB->sincos_o2[0] * localB->sincos_o2[2];

1315


1316
      /* Sum: '<S698>/Sum' */

1317
      localB->VectorConcatenate_c[4] = localB->compression + localB->prod2;

1318


1319
      /* Product: '<S701>/u(2)*u(3)*u(4)' */

1320
      localB->compression = localB->Product3[1] * localB->Product3[2] *

1321
        localB->sincos_o2[0];

1322


1323
      /* Product: '<S701>/u(1)*u(6)' */

1324
      localB->prod2 = localB->sincos_o2[2] * localB->Product3[0];

1325


1326
      /* Sum: '<S701>/Sum' */

1327
      localB->VectorConcatenate_c[5] = localB->compression - localB->prod2;

1328


1329
      /* Gain: '<S696>/Gain2' */

1330
      localB->VectorConcatenate_c[6] = -localB->Product3[1];

1331


1332
      /* Product: '<S699>/u(1)*u(3)' */

1333
      localB->VectorConcatenate_c[7] = localB->Product3[0] * localB->sincos_o2[1];

1334


1335
      /* Product: '<S702>/u(4)*u(5)' */

1336
      localB->VectorConcatenate_c[8] = localB->sincos_o2[0] * localB->sincos_o2

1337
        [1];

1338


1339
      /* Math: '<S688>/Math Function1' */

1340
      for (i = 0; i < 3; i++) {

1341
        localB->MathFunction1[3 * i] = localB->VectorConcatenate_c[i];

1342
        localB->MathFunction1[1 + 3 * i] = localB->VectorConcatenate_c[i + 3];

1343
        localB->MathFunction1[2 + 3 * i] = localB->VectorConcatenate_c[i + 6];

1344
      }

1345


1346
      /* End of Math: '<S688>/Math Function1' */

1347


1348
      /* SignalConversion: '<S688>/TmpSignal ConversionAtProduct3Inport2' incorporates:

1349
       *  Constant: '<S688>/Constant1'

1350
       */

1351
      localB->sincos_o2[0] = rtu_GearLoc[0];

1352
      localB->sincos_o2[1] = rtu_GearLoc[1];

1353
      localB->sincos_o2[2] = 0.0;

1354


1355
      /* Product: '<S688>/Product3' */

1356
      for (i = 0; i < 3; i++) {

1357
        localB->Product3[i] = 0.0;

1358
        localB->Product3[i] += localB->MathFunction1[i] * localB->sincos_o2[0];

1359
        localB->Product3[i] += localB->MathFunction1[i + 3] * localB->sincos_o2

1360
          [1];

1361
        localB->Product3[i] += localB->MathFunction1[i + 6] * localB->sincos_o2

1362
          [2];

1363
      }

1364


1365
      /* End of Product: '<S688>/Product3' */

1366


1367
      /* Sum: '<S688>/Sum5' */

1368
      localB->compression = rtu_plant - localB->Product3[2];

1369


1370
      /* Sum: '<S688>/Sum2' */

1371
      localB->compression += rtu_ground_z_m;

1372


1373
      /* Selector: '<S688>/get_row3' */

1374
      localB->sincos_o2[0] = localB->VectorConcatenate_c[2];

1375
      localB->sincos_o2[1] = localB->VectorConcatenate_c[5];

1376
      localB->sincos_o2[2] = localB->VectorConcatenate_c[8];

1377


1378
      /* Product: '<S693>/compression' */

1379
      localB->compression /= localB->sincos_o2[2];

1380


1381
      /* Sum: '<S693>/Sum1' */

1382
      localB->prod2 = rtu_GearLoc[2] - localB->compression;

1383


1384
      /* Saturate: '<S693>/RangeLimit' */

1385
      if (localB->prod2 > 0.5) {

1386
        localB->RangeLimit = 0.5;

1387
      } else if (localB->prod2 < 0.0) {

1388
        localB->RangeLimit = 0.0;

1389
      } else {

1390
        localB->RangeLimit = localB->prod2;

1391
      }

1392


1393
      /* End of Saturate: '<S693>/RangeLimit' */

1394


1395
      /* Gain: '<S693>/Gain1' */

1396
      localB->prod2 = 10000.0 * localB->RangeLimit;

1397


1398
      /* Sum: '<S693>/dCompression' */

1399
      localB->prod1 = localB->RangeLimit - localB->Xcmpr_prev;

1400


1401
      /* Gain: '<S693>/ComprRate' */

1402
      localB->prod1 *= 100.0;

1403


1404
      /* Gain: '<S693>/Gain2' */

1405
      localB->prod1 *= 10000.0;

1406


1407
      /* Sum: '<S693>/Sum2' */

1408
      localB->prod2 += localB->prod1;

1409


1410
      /* Product: '<S693>/NormalForce' */

1411
      localB->prod2 /= localB->sincos_o2[2];

1412


1413
      /* Saturate: '<S693>/ground only pushes' */

1414
      if (localB->prod2 <= 0.0) {

1415
        localB->prod2 = 0.0;

1416
      }

1417


1418
      /* End of Saturate: '<S693>/ground only pushes' */

1419


1420
      /* Gain: '<S693>/reaction' */

1421
      localB->reaction = -localB->prod2;

1422
    }

1423


1424
    if (ssIsSampleHit(S, 1, 0)) {

1425
    }

1426


1427
    if (1) {

1428
      /* Product: '<S686>/Product8' */

1429
      localB->sincos_o1[0] = localB->reaction * rtu_FricCoeff;

1430


1431
      /* Product: '<S686>/Product9' */

1432
      localB->sincos_o1[1] = localB->reaction * rtu_FricCoeff_j;

1433


1434
      /* Gain: '<S686>/Gain2' */

1435
      localB->sincos_o1[2] = localB->reaction;

1436


1437
      /* Product: '<S686>/Local Forces to Body Axes1' */

1438
      for (i = 0; i < 3; i++) {

1439
        localB->LocalForcestoBodyAxes1[i] = 0.0;

1440
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_c[i] *

1441
          localB->sincos_o1[0];

1442
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_c[i + 3] *

1443
          localB->sincos_o1[1];

1444
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_c[i + 6] *

1445
          localB->sincos_o1[2];

1446
      }

1447


1448
      /* End of Product: '<S686>/Local Forces to Body Axes1' */

1449
    }

1450


1451
    if (ssIsSampleHit(S, 1, 0)) {

1452
    }

1453


1454
    if (1) {

1455
      /* Product: '<S688>/prod1' */

1456
      localB->prod1 = rtu_GearLoc[0] * localB->sincos_o2[0];

1457


1458
      /* Product: '<S688>/prod2' */

1459
      localB->prod2 = rtu_GearLoc[1] * localB->sincos_o2[1];

1460


1461
      /* Sum: '<S688>/Sum1' */

1462
      localB->Sum1 = ((rtu_ground_z_m + rtu_plant) - localB->prod1) -

1463
        localB->prod2;

1464
    }

1465


1466
    if (ssIsSampleHit(S, 1, 0)) {

1467
      /* Constant: '<S691>/zero1' */

1468
      localB->VectorConcatenate_p[0] = 0.0;

1469
    }

1470


1471
    if (1) {

1472
      /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn2' */

1473
      localB->VectorConcatenate_p[1] = localB->compression;

1474


1475
      /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn3' */

1476
      localB->VectorConcatenate_p[2] = localC->inv3;

1477


1478
      /* Gain: '<S691>/inv1' */

1479
      localB->VectorConcatenate_p[3] = -localB->compression;

1480
    }

1481


1482
    if (ssIsSampleHit(S, 1, 0)) {

1483
      /* Constant: '<S691>/zero2' */

1484
      localB->VectorConcatenate_p[4] = 0.0;

1485
    }

1486


1487
    if (1) {

1488
      /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn6' */

1489
      localB->VectorConcatenate_p[5] = rtu_GearLoc[0];

1490


1491
      /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn7' */

1492
      localB->VectorConcatenate_p[6] = rtu_GearLoc[1];

1493


1494
      /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn8' */

1495
      localB->VectorConcatenate_p[7] = localC->inv2;

1496
    }

1497


1498
    if (ssIsSampleHit(S, 1, 0)) {

1499
      /* Constant: '<S691>/zero3' */

1500
      localB->VectorConcatenate_p[8] = 0.0;

1501
    }

1502


1503
    if (1) {

1504
      /* Product: '<S689>/force to moment' */

1505
      for (i = 0; i < 3; i++) {

1506
        localB->sincos_o1[i] = 0.0;

1507
        localB->sincos_o1[i] += localB->VectorConcatenate_p[3 * i] *

1508
          localB->LocalForcestoBodyAxes1[0];

1509
        localB->sincos_o1[i] += localB->VectorConcatenate_p[3 * i + 1] *

1510
          localB->LocalForcestoBodyAxes1[1];

1511
        localB->sincos_o1[i] += localB->VectorConcatenate_p[3 * i + 2] *

1512
          localB->LocalForcestoBodyAxes1[2];

1513
      }

1514


1515
      /* End of Product: '<S689>/force to moment' */

1516


1517
      /* Gain: '<S686>/Gain1' */

1518
      for (i = 0; i < 3; i++) {

1519
        localB->Gain1[i] = 0.0;

1520
        localB->Gain1[i] += AFCS_MODEL1_ConstP.Gain1_Gain[i] * localB->

1521
          sincos_o1[0];

1522
        localB->Gain1[i] += AFCS_MODEL1_ConstP.Gain1_Gain[i + 3] *

1523
          localB->sincos_o1[1];

1524
        localB->Gain1[i] += AFCS_MODEL1_ConstP.Gain1_Gain[i + 6] *

1525
          localB->sincos_o1[2];

1526
      }

1527


1528
      /* End of Gain: '<S686>/Gain1' */

1529


1530
      /* Trigonometry: '<S705>/sincos' */

1531
      localB->sincos_o1[0] = sin(rtu_Euler[0]);

1532
      localB->sincos_o2[0] = cos(rtu_Euler[0]);

1533
      localB->sincos_o1[1] = sin(rtu_Euler[1]);

1534
      localB->sincos_o2[1] = cos(rtu_Euler[1]);

1535
      localB->sincos_o1[2] = sin(rtu_Euler[2]);

1536
      localB->sincos_o2[2] = cos(rtu_Euler[2]);

1537


1538
      /* Fcn: '<S705>/Fcn11' */

1539
      localB->VectorConcatenate_p2[0] = localB->sincos_o2[1] * localB->

1540
        sincos_o2[0];

1541


1542
      /* Fcn: '<S705>/Fcn21' */

1543
      localB->VectorConcatenate_p2[1] = localB->sincos_o1[2] * localB->

1544
        sincos_o1[1] * localB->sincos_o2[0] - localB->sincos_o2[2] *

1545
        localB->sincos_o1[0];

1546


1547
      /* Fcn: '<S705>/Fcn31' */

1548
      localB->VectorConcatenate_p2[2] = localB->sincos_o2[2] * localB->

1549
        sincos_o1[1] * localB->sincos_o2[0] + localB->sincos_o1[2] *

1550
        localB->sincos_o1[0];

1551


1552
      /* Fcn: '<S705>/Fcn12' */

1553
      localB->VectorConcatenate_p2[3] = localB->sincos_o2[1] * localB->

1554
        sincos_o1[0];

1555


1556
      /* Fcn: '<S705>/Fcn22' */

1557
      localB->VectorConcatenate_p2[4] = localB->sincos_o1[2] * localB->

1558
        sincos_o1[1] * localB->sincos_o1[0] + localB->sincos_o2[2] *

1559
        localB->sincos_o2[0];

1560


1561
      /* Fcn: '<S705>/Fcn32' */

1562
      localB->VectorConcatenate_p2[5] = localB->sincos_o2[2] * localB->

1563
        sincos_o1[1] * localB->sincos_o1[0] - localB->sincos_o1[2] *

1564
        localB->sincos_o2[0];

1565


1566
      /* Fcn: '<S705>/Fcn13' */

1567
      localB->VectorConcatenate_p2[6] = -localB->sincos_o1[1];

1568


1569
      /* Fcn: '<S705>/Fcn23' */

1570
      localB->VectorConcatenate_p2[7] = localB->sincos_o1[2] * localB->

1571
        sincos_o2[1];

1572


1573
      /* Fcn: '<S705>/Fcn33' */

1574
      localB->VectorConcatenate_p2[8] = localB->sincos_o2[2] * localB->

1575
        sincos_o2[1];

1576
    }

1577
  }

1578


1579
  /* End of Outputs for SubSystem: '<S620>/GenerateGroundFM' */

1580
  if (1) {

1581
    /* Gain: '<S620>/Gain' */

1582
    localB->Gain = -localB->LocalForcestoBodyAxes1[2];

1583


1584
    /* SignalConversion: '<S687>/TmpSignal ConversionAtSinCosInport1' incorporates:

1585
     *  Constant: '<S620>/Constant'

1586
     */

1587
    localB->SinCos_o1[0] = rtu_plant_a;

1588
    localB->SinCos_o1[1] = rtu_plant_j;

1589
    localB->SinCos_o1[2] = 0.0;

1590


1591
    /* Trigonometry: '<S687>/SinCos' */

1592
    cosOut = cos(localB->SinCos_o1[0]);

1593
    localB->SinCos_o1[0] = sin(localB->SinCos_o1[0]);

1594
    localB->SinCos_o2[0] = cosOut;

1595
    cosOut = cos(localB->SinCos_o1[1]);

1596
    localB->SinCos_o1[1] = sin(localB->SinCos_o1[1]);

1597
    localB->SinCos_o2[1] = cosOut;

1598
    cosOut = cos(localB->SinCos_o1[2]);

1599
    localB->SinCos_o1[2] = sin(localB->SinCos_o1[2]);

1600
    localB->SinCos_o2[2] = cosOut;

1601


1602
    /* Product: '<S707>/u(5)*u(6)' */

1603
    localB->VectorConcatenate[0] = localB->SinCos_o2[1] * localB->SinCos_o2[2];

1604


1605
    /* Product: '<S708>/u(3)*u(5)' */

1606
    localB->VectorConcatenate[3] = localB->SinCos_o1[2] * localB->SinCos_o2[1];

1607


1608
    /* Gain: '<S709>/Gain2' */

1609
    localB->VectorConcatenate[6] = -localB->SinCos_o1[1];

1610


1611
    /* Product: '<S710>/u(6)*u(1)*u(2)' */

1612
    localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0] *

1613
      localB->SinCos_o1[1];

1614


1615
    /* Product: '<S710>/u(3)*u(4)' */

1616
    localB->u2u3u4 = localB->SinCos_o1[2] * localB->SinCos_o2[0];

1617


1618
    /* Sum: '<S710>/Sum' */

1619
    localB->VectorConcatenate[1] = localB->u1u6 - localB->u2u3u4;

1620


1621
    /* Product: '<S711>/u(1)*u(2)*u(3)' */

1622
    localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[1] *

1623
      localB->SinCos_o1[2];

1624


1625
    /* Product: '<S711>/u(4)*u(6)' */

1626
    localB->u1u6 = localB->SinCos_o2[0] * localB->SinCos_o2[2];

1627


1628
    /* Sum: '<S711>/Sum' */

1629
    localB->VectorConcatenate[4] = localB->u2u3u4 + localB->u1u6;

1630


1631
    /* Product: '<S712>/u(1)*u(3)' */

1632
    localB->VectorConcatenate[7] = localB->SinCos_o1[0] * localB->SinCos_o2[1];

1633


1634
    /* Product: '<S713>/u(1)*u(3)' */

1635
    localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[2];

1636


1637
    /* Product: '<S713>/u(2)*u(4)*u(6)' */

1638
    localB->u1u6 = localB->SinCos_o1[1] * localB->SinCos_o2[0] *

1639
      localB->SinCos_o2[2];

1640


1641
    /* Sum: '<S713>/Sum' */

1642
    localB->VectorConcatenate[2] = localB->u2u3u4 + localB->u1u6;

1643


1644
    /* Product: '<S714>/u(2)*u(3)*u(4)' */

1645
    localB->u2u3u4 = localB->SinCos_o1[1] * localB->SinCos_o1[2] *

1646
      localB->SinCos_o2[0];

1647


1648
    /* Product: '<S714>/u(1)*u(6)' */

1649
    localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0];

1650


1651
    /* Sum: '<S714>/Sum' */

1652
    localB->VectorConcatenate[5] = localB->u2u3u4 - localB->u1u6;

1653


1654
    /* Product: '<S715>/u(4)*u(5)' */

1655
    localB->VectorConcatenate[8] = localB->SinCos_o2[0] * localB->SinCos_o2[1];

1656


1657
    /* Switch: '<S620>/Switch' */

1658
    localB->Switch = (int8_T)(localB->Gain > 3.0);

1659
  }

1660
}

1661


1662
/* Update for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */

1663
void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Update(SimStruct * const S,

1664
  B_NoseGearGeometryForcesandMo_T *localB)

1665
{

1666
  /* Update for Enabled SubSystem: '<S620>/GenerateGroundFM' incorporates:

1667
   *  Update for EnablePort: '<S686>/Enable'

1668
   */

1669
  if (((boolean_T *)ssGetDWork(S, 628))[0] && ssIsSampleHit(S, 1, 0)) {

1670
    /* Update for Memory: '<S693>/Xcmpr_prev' */

1671
    ((real_T *)ssGetDWork(S, 624))[0] = localB->RangeLimit;

1672
  }

1673


1674
  /* End of Update for SubSystem: '<S620>/GenerateGroundFM' */

1675
}

1676


1677
/* Termination for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */

1678
void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Term(SimStruct *const S)

1679
{

1680
}

1681


1682
/* System initialize for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */

1683
void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Init(SimStruct * const S,

1684
  B_RMainGearGeometryForcesandM_T *localB)

1685
{

1686
  /* SystemInitialize for Enabled SubSystem: '<S621>/GenerateGroundFM' */

1687
  /* InitializeConditions for Memory: '<S727>/Xcmpr_prev' */

1688
  ((real_T *)ssGetDWork(S, 615))[0] = 0.0;

1689


1690
  /* SystemInitialize for Outport: '<S717>/FM' */

1691
  localB->LocalForcestoBodyAxes1[0] = 0.0;

1692
  localB->Gain2[0] = 0.0;

1693
  localB->LocalForcestoBodyAxes1[1] = 0.0;

1694
  localB->Gain2[1] = 0.0;

1695
  localB->LocalForcestoBodyAxes1[2] = 0.0;

1696
  localB->Gain2[2] = 0.0;

1697


1698
  /* SystemInitialize for Outport: '<S717>/GearCmprs' */

1699
  localB->RangeLimit = 0.0;

1700


1701
  /* End of SystemInitialize for SubSystem: '<S621>/GenerateGroundFM' */

1702
}

1703


1704
/* Disable for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */

1705
void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Disable(SimStruct * const S,

1706
  B_RMainGearGeometryForcesandM_T *localB)

1707
{

1708
  /* Disable for Enabled SubSystem: '<S621>/GenerateGroundFM' */

1709
  if (((boolean_T *)ssGetDWork(S, 623))[0]) {

1710
    /* Disable for Outport: '<S717>/FM' */

1711
    localB->LocalForcestoBodyAxes1[0] = 0.0;

1712
    localB->Gain2[0] = 0.0;

1713
    localB->LocalForcestoBodyAxes1[1] = 0.0;

1714
    localB->Gain2[1] = 0.0;

1715
    localB->LocalForcestoBodyAxes1[2] = 0.0;

1716
    localB->Gain2[2] = 0.0;

1717


1718
    /* Disable for Outport: '<S717>/GearCmprs' */

1719
    localB->RangeLimit = 0.0;

1720
    ((boolean_T *)ssGetDWork(S, 623))[0] = false;

1721
  }

1722


1723
  /* End of Disable for SubSystem: '<S621>/GenerateGroundFM' */

1724
}

1725


1726
/* Start for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */

1727
void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Start(SimStruct * const S)

1728
{

1729
  /* Start for Enabled SubSystem: '<S621>/GenerateGroundFM' */

1730
  ((boolean_T *)ssGetDWork(S, 623))[0] = false;

1731
  ssSetSolverNeedsReset(S);

1732
}

1733


1734
/* Outputs for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */

1735
void AFCS_MODEL1_RMainGearGeometryForcesandMoments(SimStruct * const S, real_T

1736
  rtu_ground_z_m, real_T rtu_plant, real_T rtu_plant_k, real_T rtu_plant_b,

1737
  const real_T rtu_GearLoc[3], real_T rtu_FricCoeff, real_T rtu_FricCoeff_k,

1738
  const real_T rtu_Euler[3], B_RMainGearGeometryForcesandM_T *localB,

1739
  ConstB_RMainGearGeometryForce_T *localC)

1740
{

1741
  real_T cosOut;

1742
  int32_T i;

1743
  if (1) {

1744
    /* Sum: '<S621>/Subtract' */

1745
    localB->Gain = rtu_plant + rtu_ground_z_m;

1746


1747
    /* Switch: '<S621>/Switch1' */

1748
    localB->Switch1 = (int8_T)!(localB->Gain > 15.0);

1749
  }

1750


1751
  if (ssIsSampleHit(S, 1, 0)) {

1752
    /* SignalConversion: '<S621>/HiddenBuf_InsertedFor_GenerateGroundFM_at_inport_5' */

1753
    localB->HiddenBuf_InsertedFor_GenerateG = localB->Switch1;

1754


1755
    /* Outputs for Enabled SubSystem: '<S621>/GenerateGroundFM' incorporates:

1756
     *  EnablePort: '<S717>/Enable'

1757
     */

1758
    if (ssIsMajorTimeStep(S)) {

1759
      if (localB->HiddenBuf_InsertedFor_GenerateG > 0) {

1760
        if (!((boolean_T *)ssGetDWork(S, 623))[0]) {

1761
          /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:

1762
           *  EnablePort: '<S651>/Enable'

1763
           */

1764
          if (ssGetTaskTime(S,1) != ssGetTStart(S)) {

1765
            ssSetSolverNeedsReset(S);

1766
          }

1767


1768
          /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */

1769
          ((boolean_T *)ssGetDWork(S, 623))[0] = true;

1770
        }

1771
      } else {

1772
        if (((boolean_T *)ssGetDWork(S, 623))[0]) {

1773
          ssSetSolverNeedsReset(S);

1774


1775
          /* Disable for Outport: '<S717>/FM' */

1776
          localB->LocalForcestoBodyAxes1[0] = 0.0;

1777
          localB->Gain2[0] = 0.0;

1778
          localB->LocalForcestoBodyAxes1[1] = 0.0;

1779
          localB->Gain2[1] = 0.0;

1780
          localB->LocalForcestoBodyAxes1[2] = 0.0;

1781
          localB->Gain2[2] = 0.0;

1782


1783
          /* Disable for Outport: '<S717>/GearCmprs' */

1784
          localB->RangeLimit = 0.0;

1785
          ((boolean_T *)ssGetDWork(S, 623))[0] = false;

1786
        }

1787
      }

1788
    }

1789


1790
    /* End of Outputs for SubSystem: '<S621>/GenerateGroundFM' */

1791
  }

1792


1793
  /* Outputs for Enabled SubSystem: '<S621>/GenerateGroundFM' incorporates:

1794
   *  EnablePort: '<S717>/Enable'

1795
   */

1796
  if (((boolean_T *)ssGetDWork(S, 623))[0]) {

1797
    if (ssIsSampleHit(S, 1, 0)) {

1798
      /* Memory: '<S727>/Xcmpr_prev' */

1799
      localB->Xcmpr_prev = ((real_T *)ssGetDWork(S, 615))[0];

1800
    }

1801


1802
    if (1) {

1803
      /* SignalConversion: '<S722>/TmpSignal ConversionAtSinCosInport1' incorporates:

1804
       *  Constant: '<S717>/Constant'

1805
       */

1806
      localB->sincos_o2[0] = rtu_plant_k;

1807
      localB->sincos_o2[1] = rtu_plant_b;

1808
      localB->sincos_o2[2] = 0.0;

1809


1810
      /* Trigonometry: '<S722>/SinCos' */

1811
      cosOut = cos(localB->sincos_o2[0]);

1812
      localB->sincos_o2[0] = sin(localB->sincos_o2[0]);

1813
      localB->sincos_o1[0] = cosOut;

1814
      cosOut = cos(localB->sincos_o2[1]);

1815
      localB->sincos_o2[1] = sin(localB->sincos_o2[1]);

1816
      localB->sincos_o1[1] = cosOut;

1817
      cosOut = cos(localB->sincos_o2[2]);

1818
      localB->sincos_o2[2] = sin(localB->sincos_o2[2]);

1819
      localB->sincos_o1[2] = cosOut;

1820


1821
      /* Product: '<S728>/u(5)*u(6)' */

1822
      localB->VectorConcatenate_o[0] = localB->sincos_o1[1] * localB->sincos_o1

1823
        [2];

1824


1825
      /* Product: '<S731>/u(6)*u(1)*u(2)' */

1826
      localB->kxi = localB->sincos_o1[2] * localB->sincos_o2[0] *

1827
        localB->sincos_o2[1];

1828


1829
      /* Product: '<S731>/u(3)*u(4)' */

1830
      localB->compression = localB->sincos_o2[2] * localB->sincos_o1[0];

1831


1832
      /* Sum: '<S731>/Sum' */

1833
      localB->VectorConcatenate_o[1] = localB->kxi - localB->compression;

1834


1835
      /* Product: '<S734>/u(1)*u(3)' */

1836
      localB->compression = localB->sincos_o2[0] * localB->sincos_o2[2];

1837


1838
      /* Product: '<S734>/u(2)*u(4)*u(6)' */

1839
      localB->kxi = localB->sincos_o2[1] * localB->sincos_o1[0] *

1840
        localB->sincos_o1[2];

1841


1842
      /* Sum: '<S734>/Sum' */

1843
      localB->VectorConcatenate_o[2] = localB->compression + localB->kxi;

1844


1845
      /* Product: '<S729>/u(3)*u(5)' */

1846
      localB->VectorConcatenate_o[3] = localB->sincos_o2[2] * localB->sincos_o1

1847
        [1];

1848


1849
      /* Product: '<S732>/u(1)*u(2)*u(3)' */

1850
      localB->compression = localB->sincos_o2[0] * localB->sincos_o2[1] *

1851
        localB->sincos_o2[2];

1852


1853
      /* Product: '<S732>/u(4)*u(6)' */

1854
      localB->kxi = localB->sincos_o1[0] * localB->sincos_o1[2];

1855


1856
      /* Sum: '<S732>/Sum' */

1857
      localB->VectorConcatenate_o[4] = localB->compression + localB->kxi;

1858


1859
      /* Product: '<S735>/u(2)*u(3)*u(4)' */

1860
      localB->compression = localB->sincos_o2[1] * localB->sincos_o2[2] *

1861
        localB->sincos_o1[0];

1862


1863
      /* Product: '<S735>/u(1)*u(6)' */

1864
      localB->kxi = localB->sincos_o1[2] * localB->sincos_o2[0];

1865


1866
      /* Sum: '<S735>/Sum' */

1867
      localB->VectorConcatenate_o[5] = localB->compression - localB->kxi;

1868


1869
      /* Gain: '<S730>/Gain2' */

1870
      localB->VectorConcatenate_o[6] = -localB->sincos_o2[1];

1871


1872
      /* Product: '<S733>/u(1)*u(3)' */

1873
      localB->VectorConcatenate_o[7] = localB->sincos_o2[0] * localB->sincos_o1

1874
        [1];

1875


1876
      /* Product: '<S736>/u(4)*u(5)' */

1877
      localB->VectorConcatenate_o[8] = localB->sincos_o1[0] * localB->sincos_o1

1878
        [1];

1879


1880
      /* Selector: '<S720>/get_row3' */

1881
      localB->sincos_o1[0] = localB->VectorConcatenate_o[2];

1882
      localB->sincos_o1[1] = localB->VectorConcatenate_o[5];

1883
      localB->sincos_o1[2] = localB->VectorConcatenate_o[8];

1884


1885
      /* Product: '<S720>/prod1' */

1886
      localB->compression = rtu_GearLoc[0] * localB->sincos_o1[0];

1887


1888
      /* Product: '<S720>/prod2' */

1889
      localB->kxi = rtu_GearLoc[1] * localB->sincos_o1[1];

1890


1891
      /* Sum: '<S720>/Sum1' */

1892
      localB->Sum1 = ((rtu_ground_z_m + rtu_plant) - localB->compression) -

1893
        localB->kxi;

1894


1895
      /* Product: '<S727>/compression' */

1896
      localB->compression = localB->Sum1 / localB->sincos_o1[2];

1897


1898
      /* Sum: '<S727>/Sum1' */

1899
      localB->kxi = rtu_GearLoc[2] - localB->compression;

1900


1901
      /* Saturate: '<S727>/RangeLimit' */

1902
      if (localB->kxi > 0.5) {

1903
        localB->RangeLimit = 0.5;

1904
      } else if (localB->kxi < 0.0) {

1905
        localB->RangeLimit = 0.0;

1906
      } else {

1907
        localB->RangeLimit = localB->kxi;

1908
      }

1909


1910
      /* End of Saturate: '<S727>/RangeLimit' */

1911


1912
      /* Gain: '<S727>/Gain1' */

1913
      localB->kxi = 10000.0 * localB->RangeLimit;

1914


1915
      /* Sum: '<S727>/dCompression' */

1916
      localB->jxk = localB->RangeLimit - localB->Xcmpr_prev;

1917


1918
      /* Gain: '<S727>/ComprRate' */

1919
      localB->jxk *= 100.0;

1920


1921
      /* Gain: '<S727>/Gain2' */

1922
      localB->jxk *= 10000.0;

1923


1924
      /* Sum: '<S727>/Sum2' */

1925
      localB->kxi += localB->jxk;

1926


1927
      /* Product: '<S727>/NormalForce' */

1928
      localB->kxi /= localB->sincos_o1[2];

1929


1930
      /* Saturate: '<S727>/ground only pushes' */

1931
      if (localB->kxi <= 0.0) {

1932
        localB->kxi = 0.0;

1933
      }

1934


1935
      /* End of Saturate: '<S727>/ground only pushes' */

1936


1937
      /* Gain: '<S727>/reaction' */

1938
      localB->kxi = -localB->kxi;

1939


1940
      /* Product: '<S717>/Product8' */

1941
      localB->GearLocalForcevector1[0] = localB->kxi * rtu_FricCoeff;

1942


1943
      /* Product: '<S717>/Product9' */

1944
      localB->GearLocalForcevector1[1] = localB->kxi * rtu_FricCoeff_k;

1945


1946
      /* SignalConversion: '<S717>/ConcatBufferAtGear  Local Force vector1In2' */

1947
      localB->GearLocalForcevector1[2] = localB->kxi;

1948


1949
      /* Product: '<S717>/Local Forces to Body Axes1' */

1950
      for (i = 0; i < 3; i++) {

1951
        localB->LocalForcestoBodyAxes1[i] = 0.0;

1952
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_o[i] *

1953
          localB->GearLocalForcevector1[0];

1954
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_o[i + 3] *

1955
          localB->GearLocalForcevector1[1];

1956
        localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_o[i + 6] *

1957
          localB->GearLocalForcevector1[2];

1958
      }

1959


1960
      /* End of Product: '<S717>/Local Forces to Body Axes1' */

1961


1962
      /* Product: '<S725>/j x k' */

1963
      localB->jxk = rtu_GearLoc[1] * localB->LocalForcestoBodyAxes1[2];

1964


1965
      /* Product: '<S725>/k x i' */

1966
      localB->kxi = rtu_GearLoc[2] * localB->LocalForcestoBodyAxes1[0];

1967


1968
      /* Product: '<S725>/i x j' */

1969
      localB->ixj = rtu_GearLoc[0] * localB->LocalForcestoBodyAxes1[1];

1970


1971
      /* Product: '<S726>/k x j' */

1972
      localB->kxj = rtu_GearLoc[2] * localB->LocalForcestoBodyAxes1[1];

1973


1974
      /* Product: '<S726>/i x k' */

1975
      localB->ixk = rtu_GearLoc[0] * localB->LocalForcestoBodyAxes1[2];

1976


1977
      /* Product: '<S726>/j x i' */

1978
      localB->jxi = rtu_GearLoc[1] * localB->LocalForcestoBodyAxes1[0];

1979


1980
      /* Sum: '<S719>/Sum' */

1981
      localB->sincos_o1[0] = localB->jxk - localB->kxj;

1982
      localB->sincos_o1[1] = localB->kxi - localB->ixk;

1983
      localB->sincos_o1[2] = localB->ixj - localB->jxi;

1984


1985
      /* Gain: '<S717>/Gain1' */

1986
      localB->Gain1[0] = -localB->sincos_o1[0];

1987
      localB->Gain1[1] = -localB->sincos_o1[1];

1988
      localB->Gain1[2] = -localB->sincos_o1[2];

1989
    }

1990


1991
    if (ssIsSampleHit(S, 1, 0)) {

1992
      /* Constant: '<S723>/zero1' */

1993
      localB->VectorConcatenate_f[0] = 0.0;

1994
    }

1995


1996
    if (1) {

1997
      /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn2' */

1998
      localB->VectorConcatenate_f[1] = localB->compression;

1999


2000
      /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn3' */

2001
      localB->VectorConcatenate_f[2] = localC->inv3;

2002


2003
      /* Gain: '<S723>/inv1' */

2004
      localB->VectorConcatenate_f[3] = -localB->compression;

2005
    }

2006


2007
    if (ssIsSampleHit(S, 1, 0)) {

2008
      /* Constant: '<S723>/zero2' */

2009
      localB->VectorConcatenate_f[4] = 0.0;

2010
    }

2011


2012
    if (1) {

2013
      /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn6' */

2014
      localB->VectorConcatenate_f[5] = rtu_GearLoc[0];

2015


2016
      /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn7' */

2017
      localB->VectorConcatenate_f[6] = rtu_GearLoc[1];

2018


2019
      /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn8' */

2020
      localB->VectorConcatenate_f[7] = localC->inv2;

2021
    }

2022


2023
    if (ssIsSampleHit(S, 1, 0)) {

2024
      /* Constant: '<S723>/zero3' */

2025
      localB->VectorConcatenate_f[8] = 0.0;

2026
    }

2027


2028
    if (1) {

2029
      /* Product: '<S721>/force to moment' */

2030
      for (i = 0; i < 3; i++) {

2031
        localB->forcetomoment[i] = 0.0;

2032
        localB->forcetomoment[i] += localB->VectorConcatenate_f[3 * i] *

2033
          localB->LocalForcestoBodyAxes1[0];

2034
        localB->forcetomoment[i] += localB->VectorConcatenate_f[3 * i + 1] *

2035
          localB->LocalForcestoBodyAxes1[1];

2036
        localB->forcetomoment[i] += localB->VectorConcatenate_f[3 * i + 2] *

2037
          localB->LocalForcestoBodyAxes1[2];

2038
      }

2039


2040
      /* End of Product: '<S721>/force to moment' */

2041
    }

2042


2043
    if (ssIsSampleHit(S, 1, 0)) {

2044
    }

2045


2046
    if (1) {

2047
      /* Math: '<S720>/Math Function1' */

2048
      for (i = 0; i < 3; i++) {

2049
        localB->MathFunction1[3 * i] = localB->VectorConcatenate_o[i];

2050
        localB->MathFunction1[1 + 3 * i] = localB->VectorConcatenate_o[i + 3];

2051
        localB->MathFunction1[2 + 3 * i] = localB->VectorConcatenate_o[i + 6];

2052
      }

2053


2054
      /* End of Math: '<S720>/Math Function1' */

2055


2056
      /* SignalConversion: '<S720>/TmpSignal ConversionAtProduct3Inport2' incorporates:

2057
       *  Constant: '<S720>/Constant1'

2058
       */

2059
      localB->sincos_o1[0] = rtu_GearLoc[0];

2060
      localB->sincos_o1[1] = rtu_GearLoc[1];

2061
      localB->sincos_o1[2] = 0.0;

2062


2063
      /* Product: '<S720>/Product3' */

2064
      for (i = 0; i < 3; i++) {

2065
        localB->Product3[i] = 0.0;

2066
        localB->Product3[i] += localB->MathFunction1[i] * localB->sincos_o1[0];

2067
        localB->Product3[i] += localB->MathFunction1[i + 3] * localB->sincos_o1

2068
          [1];

2069
        localB->Product3[i] += localB->MathFunction1[i + 6] * localB->sincos_o1

2070
          [2];

2071
      }

2072


2073
      /* End of Product: '<S720>/Product3' */

2074


2075
      /* Sum: '<S720>/Sum5' */

2076
      localB->Sum5 = rtu_plant - localB->Product3[2];

2077
    }

2078


2079
    if (ssIsSampleHit(S, 1, 0)) {

2080
    }

2081


2082
    if (1) {

2083
      /* Gain: '<S717>/Gain2' */

2084
      localB->Gain2[0] = localB->forcetomoment[0];

2085


2086
      /* Trigonometry: '<S739>/sincos' */

2087
      localB->sincos_o1[0] = sin(rtu_Euler[0]);

2088
      localB->sincos_o2[0] = cos(rtu_Euler[0]);

2089


2090
      /* Gain: '<S717>/Gain2' */

2091
      localB->Gain2[1] = -localB->forcetomoment[1];

2092


2093
      /* Trigonometry: '<S739>/sincos' */

2094
      localB->sincos_o1[1] = sin(rtu_Euler[1]);

2095
      localB->sincos_o2[1] = cos(rtu_Euler[1]);

2096


2097
      /* Gain: '<S717>/Gain2' */

2098
      localB->Gain2[2] = -localB->forcetomoment[2];

2099


2100
      /* Trigonometry: '<S739>/sincos' */

2101
      localB->sincos_o1[2] = sin(rtu_Euler[2]);

2102
      localB->sincos_o2[2] = cos(rtu_Euler[2]);

2103


2104
      /* Fcn: '<S739>/Fcn11' */

2105
      localB->VectorConcatenate_h[0] = localB->sincos_o2[1] * localB->sincos_o2

2106
        [0];

2107


2108
      /* Fcn: '<S739>/Fcn21' */

2109
      localB->VectorConcatenate_h[1] = localB->sincos_o1[2] * localB->sincos_o1

2110
        [1] * localB->sincos_o2[0] - localB->sincos_o2[2] * localB->sincos_o1[0];

2111


2112
      /* Fcn: '<S739>/Fcn31' */

2113
      localB->VectorConcatenate_h[2] = localB->sincos_o2[2] * localB->sincos_o1

2114
        [1] * localB->sincos_o2[0] + localB->sincos_o1[2] * localB->sincos_o1[0];

2115


2116
      /* Fcn: '<S739>/Fcn12' */

2117
      localB->VectorConcatenate_h[3] = localB->sincos_o2[1] * localB->sincos_o1

2118
        [0];

2119


2120
      /* Fcn: '<S739>/Fcn22' */

2121
      localB->VectorConcatenate_h[4] = localB->sincos_o1[2] * localB->sincos_o1

2122
        [1] * localB->sincos_o1[0] + localB->sincos_o2[2] * localB->sincos_o2[0];

2123


2124
      /* Fcn: '<S739>/Fcn32' */

2125
      localB->VectorConcatenate_h[5] = localB->sincos_o2[2] * localB->sincos_o1

2126
        [1] * localB->sincos_o1[0] - localB->sincos_o1[2] * localB->sincos_o2[0];

2127


2128
      /* Fcn: '<S739>/Fcn13' */

2129
      localB->VectorConcatenate_h[6] = -localB->sincos_o1[1];

2130


2131
      /* Fcn: '<S739>/Fcn23' */

2132
      localB->VectorConcatenate_h[7] = localB->sincos_o1[2] * localB->sincos_o2

2133
        [1];

2134


2135
      /* Fcn: '<S739>/Fcn33' */

2136
      localB->VectorConcatenate_h[8] = localB->sincos_o2[2] * localB->sincos_o2

2137
        [1];

2138
    }

2139
  }

2140


2141
  /* End of Outputs for SubSystem: '<S621>/GenerateGroundFM' */

2142
  if (1) {

2143
    /* Gain: '<S621>/Gain' */

2144
    localB->Gain = -localB->LocalForcestoBodyAxes1[2];

2145


2146
    /* SignalConversion: '<S718>/TmpSignal ConversionAtSinCosInport1' incorporates:

2147
     *  Constant: '<S621>/Constant'

2148
     */

2149
    localB->SinCos_o1[0] = rtu_plant_k;

2150
    localB->SinCos_o1[1] = rtu_plant_b;

2151
    localB->SinCos_o1[2] = 0.0;

2152


2153
    /* Trigonometry: '<S718>/SinCos' */

2154
    cosOut = cos(localB->SinCos_o1[0]);

2155
    localB->SinCos_o1[0] = sin(localB->SinCos_o1[0]);

2156
    localB->SinCos_o2[0] = cosOut;

2157
    cosOut = cos(localB->SinCos_o1[1]);

2158
    localB->SinCos_o1[1] = sin(localB->SinCos_o1[1]);

2159
    localB->SinCos_o2[1] = cosOut;

2160
    cosOut = cos(localB->SinCos_o1[2]);

2161
    localB->SinCos_o1[2] = sin(localB->SinCos_o1[2]);

2162
    localB->SinCos_o2[2] = cosOut;

2163


2164
    /* Product: '<S741>/u(5)*u(6)' */

2165
    localB->VectorConcatenate[0] = localB->SinCos_o2[1] * localB->SinCos_o2[2];

2166


2167
    /* Product: '<S742>/u(3)*u(5)' */

2168
    localB->VectorConcatenate[3] = localB->SinCos_o1[2] * localB->SinCos_o2[1];

2169


2170
    /* Gain: '<S743>/Gain2' */

2171
    localB->VectorConcatenate[6] = -localB->SinCos_o1[1];

2172


2173
    /* Product: '<S744>/u(6)*u(1)*u(2)' */

2174
    localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0] *

2175
      localB->SinCos_o1[1];

2176


2177
    /* Product: '<S744>/u(3)*u(4)' */

2178
    localB->u2u3u4 = localB->SinCos_o1[2] * localB->SinCos_o2[0];

2179


2180
    /* Sum: '<S744>/Sum' */

2181
    localB->VectorConcatenate[1] = localB->u1u6 - localB->u2u3u4;

2182


2183
    /* Product: '<S745>/u(1)*u(2)*u(3)' */

2184
    localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[1] *

2185
      localB->SinCos_o1[2];

2186


2187
    /* Product: '<S745>/u(4)*u(6)' */

2188
    localB->u1u6 = localB->SinCos_o2[0] * localB->SinCos_o2[2];

2189


2190
    /* Sum: '<S745>/Sum' */

2191
    localB->VectorConcatenate[4] = localB->u2u3u4 + localB->u1u6;

2192


2193
    /* Product: '<S746>/u(1)*u(3)' */

2194
    localB->VectorConcatenate[7] = localB->SinCos_o1[0] * localB->SinCos_o2[1];

2195


2196
    /* Product: '<S747>/u(1)*u(3)' */

2197
    localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[2];

2198


2199
    /* Product: '<S747>/u(2)*u(4)*u(6)' */

2200
    localB->u1u6 = localB->SinCos_o1[1] * localB->SinCos_o2[0] *

2201
      localB->SinCos_o2[2];

2202


2203
    /* Sum: '<S747>/Sum' */

2204
    localB->VectorConcatenate[2] = localB->u2u3u4 + localB->u1u6;

2205


2206
    /* Product: '<S748>/u(2)*u(3)*u(4)' */

2207
    localB->u2u3u4 = localB->SinCos_o1[1] * localB->SinCos_o1[2] *

2208
      localB->SinCos_o2[0];

2209


2210
    /* Product: '<S748>/u(1)*u(6)' */

2211
    localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0];

2212


2213
    /* Sum: '<S748>/Sum' */

2214
    localB->VectorConcatenate[5] = localB->u2u3u4 - localB->u1u6;

2215


2216
    /* Product: '<S749>/u(4)*u(5)' */

2217
    localB->VectorConcatenate[8] = localB->SinCos_o2[0] * localB->SinCos_o2[1];

2218


2219
    /* Switch: '<S621>/Switch' */

2220
    localB->Switch = (int8_T)(localB->Gain > 3.0);

2221
  }

2222
}

2223


2224
/* Update for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */

2225
void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Update(SimStruct * const S,

2226
  B_RMainGearGeometryForcesandM_T *localB)

2227
{

2228
  /* Update for Enabled SubSystem: '<S621>/GenerateGroundFM' incorporates:

2229
   *  Update for EnablePort: '<S717>/Enable'

2230
   */

2231
  if (((boolean_T *)ssGetDWork(S, 623))[0] && ssIsSampleHit(S, 1, 0)) {

2232
    /* Update for Memory: '<S727>/Xcmpr_prev' */

2233
    ((real_T *)ssGetDWork(S, 615))[0] = localB->RangeLimit;

2234
  }

2235


2236
  /* End of Update for SubSystem: '<S621>/GenerateGroundFM' */

2237
}

2238


2239
/* Termination for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */

2240
void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Term(SimStruct *const S)

2241
{

2242
}

2243


2244
static real_T AFCS_MODEL1_rt_roundd_snf(real_T u)

2245
{

2246
  real_T y;

2247
  if (fabs(u) < 4.503599627370496E+15) {

2248
    if (u >= 0.5) {

2249
      y = floor(u + 0.5);

2250
    } else if (u > -0.5) {

2251
      y = u * 0.0;

2252
    } else {

2253
      y = ceil(u - 0.5);

2254
    }

2255
  } else {

2256
    y = u;

2257
  }

2258


2259
  return y;

2260
}

2261


2262
static real_T AFCS_MODEL1_rt_remd_snf(real_T u0, real_T u1)

2263
{

2264
  real_T y;

2265
  real_T u1_0;

2266
  if (!((!rtIsNaN(u0)) && (!rtIsInf(u0)) && ((!rtIsNaN(u1)) && (!rtIsInf(u1)))))

2267
  {

2268
    y = (rtNaN);

2269
  } else {

2270
    if (u1 < 0.0) {

2271
      u1_0 = ceil(u1);

2272
    } else {

2273
      u1_0 = floor(u1);

2274
    }

2275


2276
    if ((u1 != 0.0) && (u1 != u1_0)) {

2277
      u1_0 = u0 / u1;

2278
      if (fabs(u1_0 - AFCS_MODEL1_rt_roundd_snf(u1_0)) <= DBL_EPSILON * fabs

2279
          (u1_0)) {

2280
        y = 0.0;

2281
      } else {

2282
        y = fmod(u0, u1);

2283
      }

2284
    } else {

2285
      y = fmod(u0, u1);

2286
    }

2287
  }

2288


2289
  return y;

2290
}

2291


2292
/* Function for MATLAB Function: '<S1>/MATLAB Function1' */

2293
static void AFCS_MODEL1_sind_m(real_T *x)

2294
{

2295
  int8_T n;

2296
  static real_T c_x;

2297
  static real_T absx;

2298
  if (!((!rtIsInf(*x)) && (!rtIsNaN(*x)))) {

2299
    c_x = (rtNaN);

2300
  } else {

2301
    c_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);

2302
    absx = fabs(c_x);

2303
    if (absx > 180.0) {

2304
      if (c_x > 0.0) {

2305
        c_x -= 360.0;

2306
      } else {

2307
        c_x += 360.0;

2308
      }

2309


2310
      absx = fabs(c_x);

2311
    }

2312


2313
    if (absx <= 45.0) {

2314
      c_x *= 0.017453292519943295;

2315
      n = 0;

2316
    } else if (absx <= 135.0) {

2317
      if (c_x > 0.0) {

2318
        c_x = (c_x - 90.0) * 0.017453292519943295;

2319
        n = 1;

2320
      } else {

2321
        c_x = (c_x + 90.0) * 0.017453292519943295;

2322
        n = -1;

2323
      }

2324
    } else if (c_x > 0.0) {

2325
      c_x = (c_x - 180.0) * 0.017453292519943295;

2326
      n = 2;

2327
    } else {

2328
      c_x = (c_x + 180.0) * 0.017453292519943295;

2329
      n = -2;

2330
    }

2331


2332
    if (n == 0) {

2333
      c_x = sin(c_x);

2334
    } else if (n == 1) {

2335
      c_x = cos(c_x);

2336
    } else if (n == -1) {

2337
      c_x = -cos(c_x);

2338
    } else {

2339
      c_x = -sin(c_x);

2340
    }

2341
  }

2342


2343
  *x = c_x;

2344
}

2345


2346
/* Function for MATLAB Function: '<S1>/MATLAB Function1' */

2347
static void AFCS_MODEL1_cosd_c(real_T *x)

2348
{

2349
  int8_T n;

2350
  static real_T b_x;

2351
  static real_T absx;

2352
  if (!((!rtIsInf(*x)) && (!rtIsNaN(*x)))) {

2353
    *x = (rtNaN);

2354
  } else {

2355
    b_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);

2356
    absx = fabs(b_x);

2357
    if (absx > 180.0) {

2358
      if (b_x > 0.0) {

2359
        b_x -= 360.0;

2360
      } else {

2361
        b_x += 360.0;

2362
      }

2363


2364
      absx = fabs(b_x);

2365
    }

2366


2367
    if (absx <= 45.0) {

2368
      b_x *= 0.017453292519943295;

2369
      n = 0;

2370
    } else if (absx <= 135.0) {

2371
      if (b_x > 0.0) {

2372
        b_x = (b_x - 90.0) * 0.017453292519943295;

2373
        n = 1;

2374
      } else {

2375
        b_x = (b_x + 90.0) * 0.017453292519943295;

2376
        n = -1;

2377
      }

2378
    } else if (b_x > 0.0) {

2379
      b_x = (b_x - 180.0) * 0.017453292519943295;

2380
      n = 2;

2381
    } else {

2382
      b_x = (b_x + 180.0) * 0.017453292519943295;

2383
      n = -2;

2384
    }

2385


2386
    if (n == 0) {

2387
      *x = cos(b_x);

2388
    } else if (n == 1) {

2389
      *x = -sin(b_x);

2390
    } else if (n == -1) {

2391
      *x = sin(b_x);

2392
    } else {

2393
      *x = -cos(b_x);

2394
    }

2395
  }

2396
}

2397


2398
static real_T AFCS_MODEL1_rt_atan2d_snf(real_T u0, real_T u1)

2399
{

2400
  real_T y;

2401
  int32_T u0_0;

2402
  int32_T u1_0;

2403
  if (rtIsNaN(u0) || rtIsNaN(u1)) {

2404
    y = (rtNaN);

2405
  } else if (rtIsInf(u0) && rtIsInf(u1)) {

2406
    if (u0 > 0.0) {

2407
      u0_0 = 1;

2408
    } else {

2409
      u0_0 = -1;

2410
    }

2411


2412
    if (u1 > 0.0) {

2413
      u1_0 = 1;

2414
    } else {

2415
      u1_0 = -1;

2416
    }

2417


2418
    y = atan2(u0_0, u1_0);

2419
  } else if (u1 == 0.0) {

2420
    if (u0 > 0.0) {

2421
      y = RT_PI / 2.0;

2422
    } else if (u0 < 0.0) {

2423
      y = -(RT_PI / 2.0);

2424
    } else {

2425
      y = 0.0;

2426
    }

2427
  } else {

2428
    y = atan2(u0, u1);

2429
  }

2430


2431
  return y;

2432
}

2433


2434
static real_T AFCS_MODEL1_rt_modd_snf(real_T u0, real_T u1)

2435
{

2436
  real_T y;

2437
  real_T tmp;

2438
  if (u1 == 0.0) {

2439
    y = u0;

2440
  } else if (!((!rtIsNaN(u0)) && (!rtIsInf(u0)) && ((!rtIsNaN(u1)) && (!rtIsInf

2441
                (u1))))) {

2442
    y = (rtNaN);

2443
  } else {

2444
    tmp = u0 / u1;

2445
    if (u1 <= floor(u1)) {

2446
      y = u0 - floor(tmp) * u1;

2447
    } else {

2448
      if (fabs(tmp - AFCS_MODEL1_rt_roundd_snf(tmp)) <= DBL_EPSILON * fabs(tmp))

2449
      {

2450
        y = 0.0;

2451
      } else {

2452
        y = (tmp - floor(tmp)) * u1;

2453
      }

2454
    }

2455
  }

2456


2457
  return y;

2458
}

2459


2460
static real32_T AFCS_MODEL1_rt_atan2f_snf(real32_T u0, real32_T u1)

2461
{

2462
  real32_T y;

2463
  int32_T u0_0;

2464
  int32_T u1_0;

2465
  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

2466
    y = (rtNaNF);

2467
  } else if (rtIsInfF(u0) && rtIsInfF(u1)) {

2468
    if (u0 > 0.0F) {

2469
      u0_0 = 1;

2470
    } else {

2471
      u0_0 = -1;

2472
    }

2473


2474
    if (u1 > 0.0F) {

2475
      u1_0 = 1;

2476
    } else {

2477
      u1_0 = -1;

2478
    }

2479


2480
    y = (real32_T)atan2((real32_T)u0_0, (real32_T)u1_0);

2481
  } else if (u1 == 0.0F) {

2482
    if (u0 > 0.0F) {

2483
      y = RT_PIF / 2.0F;

2484
    } else if (u0 < 0.0F) {

2485
      y = -(RT_PIF / 2.0F);

2486
    } else {

2487
      y = 0.0F;

2488
    }

2489
  } else {

2490
    y = (real32_T)atan2(u0, u1);

2491
  }

2492


2493
  return y;

2494
}

2495


2496
/* Function for Chart: '<S21>/delayblock3' */

2497
static int32_T AFCS_MODEL1_addup_g(int32_T bA, int32_T iB, int32_T iC)

2498
{

2499
  int32_T tC;

2500
  int64_T tmp;

2501


2502
  /* MATLAB Function 'addup': '<S50>:114' */

2503
  /* Graphical Function 'addup': '<S50>:114' */

2504
  /* '<S50>:122:1' */

2505
  tC = iC;

2506


2507
  /* '<S50>:123:1' */

2508
  if (bA == 0) {

2509
    /* '<S50>:126:1' */

2510
    if (iC > 0) {

2511
      /* '<S50>:126:1' */

2512
      tC = iC - 1;

2513
    }

2514
  } else {

2515
    /* '<S50>:128:1' */

2516
    if (iC > iB) {

2517
      /* '<S50>:128:1' */

2518
      tC = iB;

2519
    } else {

2520
      /* '<S50>:127:1' */

2521
      tmp = iC + 1LL;

2522
      if (tmp > 2147483647LL) {

2523
        tmp = 2147483647LL;

2524
      } else {

2525
        if (tmp < -2147483648LL) {

2526
          tmp = -2147483648LL;

2527
        }

2528
      }

2529


2530
      tC = (int32_T)tmp;

2531
    }

2532
  }

2533


2534
  return tC;

2535
}

2536


2537
/* Function for MATLAB Function: '<S293>/MATLAB Function3' */

2538
static void AFCS_MODEL1_GetPsi(real_T A_lon, real_T A_lat, real_T B_lon, real_T

2539
  B_lat, real_T *Drange, real_T *PsiNorth)

2540
{

2541
  static real_T lona;

2542
  static real_T lata;

2543
  static real_T lonb;

2544
  static real_T latb;

2545
  static real_T a;

2546
  real_T b_a;

2547


2548
  /* '<S298>:1:53' */

2549
  /* '<S298>:1:54' */

2550
  /* '<S298>:1:55' */

2551
  lona = A_lon * 0.017453292519943295;

2552


2553
  /* '<S298>:1:56' */

2554
  lata = A_lat * 0.017453292519943295;

2555


2556
  /* '<S298>:1:57' */

2557
  lonb = B_lon * 0.017453292519943295;

2558


2559
  /* '<S298>:1:58' */

2560
  latb = B_lat * 0.017453292519943295;

2561


2562
  /* '<S298>:1:59' */

2563
  a = latb - lata;

2564
  b_a = cos((lata + latb) * 0.5) * (lonb - lona);

2565


2566
  /* '<S298>:1:60' */

2567
  *Drange = sqrt(a * a + b_a * b_a) * 6.371E+6;

2568


2569
  /* %%%%%%%%%%%%%%%计算AB与北向夹角 */

2570
  /* '<S298>:1:62' */

2571
  /* '<S298>:1:63' */

2572
  /* '<S298>:1:64' */

2573
  *PsiNorth = AFCS_MODEL1_rt_atan2d_snf(cos((latb + lata) * 0.5) * (lonb - lona),

2574
    latb - lata);

2575
}

2576


2577
static real_T AFCS_MODEL1_rt_powd_snf(real_T u0, real_T u1)

2578
{

2579
  real_T y;

2580
  real_T tmp;

2581
  real_T tmp_0;

2582
  if (rtIsNaN(u0) || rtIsNaN(u1)) {

2583
    y = (rtNaN);

2584
  } else {

2585
    tmp = fabs(u0);

2586
    tmp_0 = fabs(u1);

2587
    if (rtIsInf(u1)) {

2588
      if (tmp == 1.0) {

2589
        y = (rtNaN);

2590
      } else if (tmp > 1.0) {

2591
        if (u1 > 0.0) {

2592
          y = (rtInf);

2593
        } else {

2594
          y = 0.0;

2595
        }

2596
      } else if (u1 > 0.0) {

2597
        y = 0.0;

2598
      } else {

2599
        y = (rtInf);

2600
      }

2601
    } else if (tmp_0 == 0.0) {

2602
      y = 1.0;

2603
    } else if (tmp_0 == 1.0) {

2604
      if (u1 > 0.0) {

2605
        y = u0;

2606
      } else {

2607
        y = 1.0 / u0;

2608
      }

2609
    } else if (u1 == 2.0) {

2610
      y = u0 * u0;

2611
    } else if ((u1 == 0.5) && (u0 >= 0.0)) {

2612
      y = sqrt(u0);

2613
    } else if ((u0 < 0.0) && (u1 > floor(u1))) {

2614
      y = (rtNaN);

2615
    } else {

2616
      y = pow(u0, u1);

2617
    }

2618
  }

2619


2620
  return y;

2621
}

2622


2623
/* Function for Chart: '<S21>/delayblock2' */

2624
static int32_T AFCS_MODEL1_addup_l(int32_T bA, int32_T iB, int32_T iC)

2625
{

2626
  int32_T tC;

2627
  int64_T tmp;

2628


2629
  /* MATLAB Function 'addup': '<S49>:114' */

2630
  /* Graphical Function 'addup': '<S49>:114' */

2631
  /* '<S49>:122:1' */

2632
  tC = iC;

2633


2634
  /* '<S49>:123:1' */

2635
  if (bA == 0) {

2636
    /* '<S49>:126:1' */

2637
    if (iC > 0) {

2638
      /* '<S49>:126:1' */

2639
      tC = iC - 1;

2640
    }

2641
  } else {

2642
    /* '<S49>:128:1' */

2643
    if (iC > iB) {

2644
      /* '<S49>:128:1' */

2645
      tC = iB;

2646
    } else {

2647
      /* '<S49>:127:1' */

2648
      tmp = iC + 1LL;

2649
      if (tmp > 2147483647LL) {

2650
        tmp = 2147483647LL;

2651
      } else {

2652
        if (tmp < -2147483648LL) {

2653
          tmp = -2147483648LL;

2654
        }

2655
      }

2656


2657
      tC = (int32_T)tmp;

2658
    }

2659
  }

2660


2661
  return tC;

2662
}

2663


2664
/* Function for Chart: '<S32>/wowcmp1' */

2665
static int32_T AFCS_MODEL1_addup_p(boolean_T bA, int32_T iB, int32_T iC)

2666
{

2667
  int32_T tC;

2668
  int64_T tmp;

2669


2670
  /* MATLAB Function 'addup': '<S331>:114' */

2671
  /* Graphical Function 'addup': '<S331>:114' */

2672
  /* '<S331>:122:1' */

2673
  tC = iC;

2674


2675
  /* '<S331>:123:1' */

2676
  if (!bA) {

2677
    /* '<S331>:126:1' */

2678
    if (iC > 0) {

2679
      /* '<S331>:126:1' */

2680
      tC = iC - 1;

2681
    }

2682
  } else {

2683
    /* '<S331>:128:1' */

2684
    if (iC > iB) {

2685
      /* '<S331>:128:1' */

2686
      tC = iB;

2687
    } else {

2688
      /* '<S331>:127:1' */

2689
      tmp = iC + 1LL;

2690
      if (tmp > 2147483647LL) {

2691
        tmp = 2147483647LL;

2692
      } else {

2693
        if (tmp < -2147483648LL) {

2694
          tmp = -2147483648LL;

2695
        }

2696
      }

2697


2698
      tC = (int32_T)tmp;

2699
    }

2700
  }

2701


2702
  return tC;

2703
}

2704


2705
/* Function for Chart: '<S21>/delayblock4' */

2706
static int32_T AFCS_MODEL1_addup_i(int32_T bA, int32_T iB, int32_T iC)

2707
{

2708
  int32_T tC;

2709
  int64_T tmp;

2710


2711
  /* MATLAB Function 'addup': '<S51>:114' */

2712
  /* Graphical Function 'addup': '<S51>:114' */

2713
  /* '<S51>:122:1' */

2714
  tC = iC;

2715


2716
  /* '<S51>:123:1' */

2717
  if (bA == 0) {

2718
    /* '<S51>:126:1' */

2719
    if (iC > 0) {

2720
      /* '<S51>:126:1' */

2721
      tC = iC - 1;

2722
    }

2723
  } else {

2724
    /* '<S51>:128:1' */

2725
    if (iC > iB) {

2726
      /* '<S51>:128:1' */

2727
      tC = iB;

2728
    } else {

2729
      /* '<S51>:127:1' */

2730
      tmp = iC + 1LL;

2731
      if (tmp > 2147483647LL) {

2732
        tmp = 2147483647LL;

2733
      } else {

2734
        if (tmp < -2147483648LL) {

2735
          tmp = -2147483648LL;

2736
        }

2737
      }

2738


2739
      tC = (int32_T)tmp;

2740
    }

2741
  }

2742


2743
  return tC;

2744
}

2745


2746
/* Function for Chart: '<S21>/delayblock5' */

2747
static int32_T AFCS_MODEL1_addup_k(int32_T bA, int32_T iB, int32_T iC)

2748
{

2749
  int32_T tC;

2750
  int64_T tmp;

2751


2752
  /* MATLAB Function 'addup': '<S52>:114' */

2753
  /* Graphical Function 'addup': '<S52>:114' */

2754
  /* '<S52>:122:1' */

2755
  tC = iC;

2756


2757
  /* '<S52>:123:1' */

2758
  if (bA == 0) {

2759
    /* '<S52>:126:1' */

2760
    if (iC > 0) {

2761
      /* '<S52>:126:1' */

2762
      tC = iC - 1;

2763
    }

2764
  } else {

2765
    /* '<S52>:128:1' */

2766
    if (iC > iB) {

2767
      /* '<S52>:128:1' */

2768
      tC = iB;

2769
    } else {

2770
      /* '<S52>:127:1' */

2771
      tmp = iC + 1LL;

2772
      if (tmp > 2147483647LL) {

2773
        tmp = 2147483647LL;

2774
      } else {

2775
        if (tmp < -2147483648LL) {

2776
          tmp = -2147483648LL;

2777
        }

2778
      }

2779


2780
      tC = (int32_T)tmp;

2781
    }

2782
  }

2783


2784
  return tC;

2785
}

2786


2787
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

2788
static void AFCS_MODEL1_TransformLLATBDX(real_T Lon, real_T Lat, real_T Alt,

2789
  real_T Lon0, real_T Lat0, real_T Alt0, real_T *pn, real_T *pe, real_T *pd)

2790
{

2791
  static real_T m_dE_LN[9];

2792
  static real_T A[3];

2793
  int32_T i;

2794
  static real_T Alt_idx_0;

2795
  static real_T Alt_idx_1;

2796
  real_T Alt_idx_2;

2797


2798
  /* TransformLLATBDX 此处显示有关此函数的摘要 */

2799
  /*    将经纬高转换到北东下坐标系 */

2800
  /*  //1.将经纬高转换为在地心坐标系下x,y,z坐标 */

2801
  /* TransformLLATDX 此处显示有关此函数的摘要 */

2802
  /*    将经纬高转换到地心坐标系 */

2803
  /*  	//2.将地心坐标系下x,y,z坐标转换为在北天东标系下x,y,z坐标 */

2804
  /* UNTITLED4 此处显示有关此函数的摘要 */

2805
  /*    将地心坐标系转换到北天东系 */

2806
  /* 地心系到北天东系转换矩阵 */

2807
  /* TransformLLATDX 此处显示有关此函数的摘要 */

2808
  /*    将经纬高转换到地心坐标系 */

2809
  m_dE_LN[0] = -sin(Lat0) * cos(Lon0);

2810
  m_dE_LN[3] = -sin(Lat0) * sin(Lon0);

2811
  m_dE_LN[6] = cos(Lat0);

2812
  m_dE_LN[1] = cos(Lat0) * cos(Lon0);

2813
  m_dE_LN[4] = cos(Lat0) * sin(Lon0);

2814
  m_dE_LN[7] = sin(Lat0);

2815
  m_dE_LN[2] = -sin(Lon0);

2816
  m_dE_LN[5] = cos(Lon0);

2817
  m_dE_LN[8] = 0.0;

2818
  Alt_idx_0 = (Alt + 6.378137E+6) * cos(Lat) * cos(Lon) - (Alt0 + 6.378137E+6) *

2819
    cos(Lat0) * cos(Lon0);

2820
  Alt_idx_1 = (Alt + 6.378137E+6) * cos(Lat) * sin(Lon) - (Alt0 + 6.378137E+6) *

2821
    cos(Lat0) * sin(Lon0);

2822
  Alt_idx_2 = (Alt + 6.378137E+6) * sin(Lat) - (Alt0 + 6.378137E+6) * sin(Lat0);

2823
  for (i = 0; i < 3; i++) {

2824
    A[i] = m_dE_LN[i + 6] * Alt_idx_2 + (m_dE_LN[i + 3] * Alt_idx_1 + m_dE_LN[i]

2825
      * Alt_idx_0);

2826
  }

2827


2828
  /*  	//3.将北天东坐标系下x,y,z坐标转换为在北东下标系下x,y,z坐标 */

2829
  /* UNTITLED6 此处显示有关此函数的摘要 */

2830
  /*   将北天东系转换到北东下坐标系 */

2831
  *pn = A[0];

2832
  *pe = A[2];

2833
  *pd = -A[1];

2834
}

2835


2836
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

2837
static void AFCS_MODEL1_linspace(real_T y[32])

2838
{

2839
  memset(&y[0], 0, sizeof(real_T) << 5U);

2840
}

2841


2842
/* Function for Chart: '<S21>/delayblock7' */

2843
static int32_T AFCS_MODEL1_addup_h(int32_T bA, int32_T iB, int32_T iC)

2844
{

2845
  int32_T tC;

2846
  int64_T tmp;

2847


2848
  /* MATLAB Function 'addup': '<S54>:114' */

2849
  /* Graphical Function 'addup': '<S54>:114' */

2850
  /* '<S54>:122:1' */

2851
  tC = iC;

2852


2853
  /* '<S54>:123:1' */

2854
  if (bA == 0) {

2855
    /* '<S54>:126:1' */

2856
    if (iC > 0) {

2857
      /* '<S54>:126:1' */

2858
      tC = iC - 1;

2859
    }

2860
  } else {

2861
    /* '<S54>:128:1' */

2862
    if (iC > iB) {

2863
      /* '<S54>:128:1' */

2864
      tC = iB;

2865
    } else {

2866
      /* '<S54>:127:1' */

2867
      tmp = iC + 1LL;

2868
      if (tmp > 2147483647LL) {

2869
        tmp = 2147483647LL;

2870
      } else {

2871
        if (tmp < -2147483648LL) {

2872
          tmp = -2147483648LL;

2873
        }

2874
      }

2875


2876
      tC = (int32_T)tmp;

2877
    }

2878
  }

2879


2880
  return tC;

2881
}

2882


2883
static real32_T AFCS_MODEL1_rt_roundf_snf(real32_T u)

2884
{

2885
  real32_T y;

2886
  if ((real32_T)fabs(u) < 8.388608E+6F) {

2887
    if (u >= 0.5F) {

2888
      y = (real32_T)floor(u + 0.5F);

2889
    } else if (u > -0.5F) {

2890
      y = u * 0.0F;

2891
    } else {

2892
      y = (real32_T)ceil(u - 0.5F);

2893
    }

2894
  } else {

2895
    y = u;

2896
  }

2897


2898
  return y;

2899
}

2900


2901
static real32_T AFCS_MODEL1_rt_remf_snf(real32_T u0, real32_T u1)

2902
{

2903
  real32_T y;

2904
  real32_T u1_0;

2905
  if (!((!rtIsNaNF(u0)) && (!rtIsInfF(u0)) && ((!rtIsNaNF(u1)) && (!rtIsInfF(u1)))))

2906
  {

2907
    y = (rtNaNF);

2908
  } else {

2909
    if (u1 < 0.0F) {

2910
      u1_0 = (real32_T)ceil(u1);

2911
    } else {

2912
      u1_0 = (real32_T)floor(u1);

2913
    }

2914


2915
    if ((u1 != 0.0F) && (u1 != u1_0)) {

2916
      u1_0 = u0 / u1;

2917
      if ((real32_T)fabs(u1_0 - AFCS_MODEL1_rt_roundf_snf(u1_0)) <= FLT_EPSILON *

2918
          (real32_T)fabs(u1_0)) {

2919
        y = 0.0F;

2920
      } else {

2921
        y = (real32_T)fmod(u0, u1);

2922
      }

2923
    } else {

2924
      y = (real32_T)fmod(u0, u1);

2925
    }

2926
  }

2927


2928
  return y;

2929
}

2930


2931
/* Function for Chart: '<S321>/Takeoff' */

2932
static int32_T AFCS_MODEL1_addup_lc(boolean_T bA, int32_T iB, int32_T iC)

2933
{

2934
  int32_T tC;

2935
  int64_T tmp;

2936


2937
  /* MATLAB Function 'addup': '<S329>:121' */

2938
  /* Graphical Function 'addup': '<S329>:121' */

2939
  /* '<S329>:129:1' */

2940
  tC = iC;

2941


2942
  /* '<S329>:130:1' */

2943
  if (!bA) {

2944
    /* '<S329>:133:1' */

2945
    if (iC > 0) {

2946
      /* '<S329>:133:1' */

2947
      tC = iC - 1;

2948
    }

2949
  } else {

2950
    /* '<S329>:135:1' */

2951
    if (iC > iB) {

2952
      /* '<S329>:135:1' */

2953
      tC = iB;

2954
    } else {

2955
      /* '<S329>:134:1' */

2956
      tmp = iC + 1LL;

2957
      if (tmp > 2147483647LL) {

2958
        tmp = 2147483647LL;

2959
      } else {

2960
        if (tmp < -2147483648LL) {

2961
          tmp = -2147483648LL;

2962
        }

2963
      }

2964


2965
      tC = (int32_T)tmp;

2966
    }

2967
  }

2968


2969
  return tC;

2970
}

2971


2972
/* Function for Chart: '<S321>/Takeoff' */

2973
static void AFCS_MODEL1_NormalTakeoff(SimStruct *S)

2974
{

2975
  boolean_T StopTakeoff1FG;

2976
  int8_T n;

2977
  real32_T b_x;

2978
  real32_T absx;

2979
  boolean_T guard1 = false;

2980
  boolean_T guard2 = false;

2981
  B_AFCS_MODEL1_T *_rtB;

2982
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

2983


2984
  /* During 'NormalTakeoff': '<S329>:136' */

2985
  if (((int32_T *)ssGetDWork(S, 326))[0] == 5) {

2986
    /* Transition: '<S329>:151' */

2987
    /* Exit Internal 'NormalTakeoff': '<S329>:136' */

2988
    ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

2989
    ((uint8_T *)ssGetDWork(S, 578))[0] = AFCS_MODEL1_IN_AbortTakeoff;

2990


2991
    /* Entry 'AbortTakeoff': '<S329>:152' */

2992
    _rtB->fmTOtoTaxi = 1;

2993
    _rtB->AbortTakeoffFg = 1;

2994
  } else {

2995
    if (_rtB->Vc_p < 30.0F) {

2996
      if ((real32_T)fabs(_rtB->yrwcmd_m - _rtB->yrw) > 8.0F) {

2997
        guard1 = true;

2998
      } else {

2999
        if (!((!rtIsInfF(_rtB->psd)) && (!rtIsNaNF(_rtB->psd)))) {

3000
          b_x = (rtNaNF);

3001
        } else {

3002
          b_x = AFCS_MODEL1_rt_remf_snf(_rtB->psd, 360.0F);

3003
          absx = (real32_T)fabs(b_x);

3004
          if (absx > 180.0F) {

3005
            if (b_x > 0.0F) {

3006
              b_x -= 360.0F;

3007
            } else {

3008
              b_x += 360.0F;

3009
            }

3010


3011
            absx = (real32_T)fabs(b_x);

3012
          }

3013


3014
          if (absx <= 45.0F) {

3015
            b_x *= 0.0174532924F;

3016
            n = 0;

3017
          } else if (absx <= 135.0F) {

3018
            if (b_x > 0.0F) {

3019
              b_x = (b_x - 90.0F) * 0.0174532924F;

3020
              n = 1;

3021
            } else {

3022
              b_x = (b_x + 90.0F) * 0.0174532924F;

3023
              n = -1;

3024
            }

3025
          } else if (b_x > 0.0F) {

3026
            b_x = (b_x - 180.0F) * 0.0174532924F;

3027
            n = 2;

3028
          } else {

3029
            b_x = (b_x + 180.0F) * 0.0174532924F;

3030
            n = -2;

3031
          }

3032


3033
          if (n == 0) {

3034
            b_x = (real32_T)sin(b_x);

3035
          } else if (n == 1) {

3036
            b_x = (real32_T)cos(b_x);

3037
          } else if (n == -1) {

3038
            b_x = -(real32_T)cos(b_x);

3039
          } else {

3040
            b_x = -(real32_T)sin(b_x);

3041
          }

3042
        }

3043


3044
        if (((real32_T)fabs(_rtB->vt * b_x) > 3.0F) || (_rtB->Switch3 == 1)) {

3045
          guard1 = true;

3046
        } else {

3047
          guard2 = true;

3048
        }

3049
      }

3050
    } else {

3051
      guard2 = true;

3052
    }

3053


3054
    if (guard2) {

3055
      /* Constant: '<S17>/Constant18' */

3056
      if (_rtB->DtoStart > 1000.0F) {

3057
        guard1 = true;

3058
      } else {

3059
        StopTakeoff1FG = false;

3060
      }

3061


3062
      /* End of Constant: '<S17>/Constant18' */

3063
    }

3064


3065
    if (guard1) {

3066
      StopTakeoff1FG = true;

3067
    }

3068


3069
    ((int32_T *)ssGetDWork(S, 326))[0] = AFCS_MODEL1_addup_lc(StopTakeoff1FG, 5,

3070
      ((int32_T *)ssGetDWork(S, 326))[0]);

3071
    switch (((uint8_T *)ssGetDWork(S, 579))[0]) {

3072
     case AFCS_MODEL1_IN_DrControl:

3073
      /* During 'DrControl': '<S329>:144' */

3074
      if (((int32_T *)ssGetDWork(S, 322))[0] == 5) {

3075
        /* Transition: '<S329>:138' */

3076
        ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_LiftFrontWheel;

3077


3078
        /* Entry 'LiftFrontWheel': '<S329>:139' */

3079
        _rtB->apthL_i2 = 1;

3080
        _rtB->aplonL_ik = 2;

3081
        _rtB->aplatL_d = 2;

3082
        _rtB->apyawL_g = 3;

3083
        _rtB->apnwsL_g = 2;

3084
        _rtB->phiLcmd_h = 0.0F;

3085


3086
        /* Constant: '<S17>/Constant1' */

3087
        _rtB->thetaLcmd_ae = 7.0F;

3088
        ((int32_T *)ssGetDWork(S, 324))[0] = 0;

3089
        _rtB->LiftWheelFg = 1;

3090
      } else {

3091
        /* Constant: '<S17>/Constant8' */

3092
        ((int32_T *)ssGetDWork(S, 322))[0] = AFCS_MODEL1_addup_lc(_rtB->Vc_p >

3093
          30.0F, 5, ((int32_T *)ssGetDWork(S, 322))[0]);

3094
      }

3095
      break;

3096


3097
     case AFCS_MODEL1_IN_Initial:

3098
      /* During 'Initial': '<S329>:193' */

3099
      if (((int32_T *)ssGetDWork(S, 327))[0] == 5) {

3100
        /* Transition: '<S329>:142' */

3101
        ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_NwsControl;

3102


3103
        /* Entry 'NwsControl': '<S329>:143' */

3104
        _rtB->apthL_i2 = 1;

3105
        _rtB->aplonL_ik = 2;

3106
        _rtB->aplatL_d = 2;

3107
        _rtB->apyawL_g = 1;

3108
        _rtB->apnwsL_g = 2;

3109
        _rtB->phiLcmd_h = 0.0F;

3110
        _rtB->thetaLcmd_ae = -10.0F;

3111
        ((int32_T *)ssGetDWork(S, 323))[0] = 0;

3112
      } else {

3113
        ((int32_T *)ssGetDWork(S, 327))[0] = AFCS_MODEL1_addup_lc(_rtB->vt >

3114
          0.5F, 5, ((int32_T *)ssGetDWork(S, 327))[0]);

3115
      }

3116
      break;

3117


3118
     case AFCS_MODEL1_IN_LiftFrontWheel:

3119
      /* During 'LiftFrontWheel': '<S329>:139' */

3120
      if (((int32_T *)ssGetDWork(S, 324))[0] == 5) {

3121
        /* Transition: '<S329>:149' */

3122
        ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_StartLatControl;

3123


3124
        /* Entry 'StartLatControl': '<S329>:167' */

3125
        _rtB->apthL_i2 = 1;

3126
        _rtB->aplonL_ik = 2;

3127
        _rtB->aplatL_d = 5;

3128
        _rtB->apyawL_g = 1;

3129
        _rtB->apnwsL_g = 1;

3130
        _rtB->phiLcmd_h = 0.0F;

3131
        _rtB->thetaLcmd_ae = 7.0F;

3132
        ((int32_T *)ssGetDWork(S, 325))[0] = 0;

3133
      } else {

3134
        ((int32_T *)ssGetDWork(S, 324))[0] = AFCS_MODEL1_addup_lc(_rtB->mh_j >

3135
          8.0F, 5, ((int32_T *)ssGetDWork(S, 324))[0]);

3136
      }

3137
      break;

3138


3139
     case AFCS_MODEL1_IN_NwsControl:

3140
      /* During 'NwsControl': '<S329>:143' */

3141
      if (((int32_T *)ssGetDWork(S, 323))[0] == 5) {

3142
        /* Transition: '<S329>:141' */

3143
        ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_DrControl;

3144


3145
        /* Entry 'DrControl': '<S329>:144' */

3146
        _rtB->apthL_i2 = 1;

3147
        _rtB->aplonL_ik = 2;

3148
        _rtB->aplatL_d = 2;

3149
        _rtB->apyawL_g = 3;

3150
        _rtB->apnwsL_g = 2;

3151
        _rtB->phiLcmd_h = 0.0F;

3152
        _rtB->thetaLcmd_ae = -2.0F;

3153
        ((int32_T *)ssGetDWork(S, 322))[0] = 0;

3154
      } else {

3155
        /* Constant: '<S17>/Constant7' */

3156
        ((int32_T *)ssGetDWork(S, 323))[0] = AFCS_MODEL1_addup_lc(_rtB->Vc_p >

3157
          5.0F, 5, ((int32_T *)ssGetDWork(S, 323))[0]);

3158
      }

3159
      break;

3160


3161
     case AFCS_MODEL1_IN_StartLatControl:

3162
      /* During 'StartLatControl': '<S329>:167' */

3163
      if (((int32_T *)ssGetDWork(S, 325))[0] == 5) {

3164
        /* Transition: '<S329>:168' */

3165
        ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_TransClimb;

3166


3167
        /* Entry 'TransClimb': '<S329>:140' */

3168
        _rtB->fmTOtoClimb = 1;

3169
      } else {

3170
        ((int32_T *)ssGetDWork(S, 325))[0] = AFCS_MODEL1_addup_lc(_rtB->mh_j >

3171
          20.0F, 5, ((int32_T *)ssGetDWork(S, 325))[0]);

3172
      }

3173
      break;

3174


3175
     default:

3176
      /* During 'TransClimb': '<S329>:140' */

3177
      break;

3178
    }

3179
  }

3180
}

3181


3182
/* Function for Chart: '<S316>/Landing2' */

3183
static int32_T AFCS_MODEL1_addup_jz(boolean_T bA, int32_T iB, int32_T iC)

3184
{

3185
  int32_T tC;

3186
  int64_T tmp;

3187


3188
  /* MATLAB Function 'addup': '<S326>:251' */

3189
  /* Graphical Function 'addup': '<S326>:251' */

3190
  /* '<S326>:259:1' */

3191
  tC = iC;

3192


3193
  /* '<S326>:260:1' */

3194
  if (!bA) {

3195
    /* '<S326>:263:1' */

3196
    if (iC > 0) {

3197
      /* '<S326>:263:1' */

3198
      tC = iC - 1;

3199
    }

3200
  } else {

3201
    /* '<S326>:265:1' */

3202
    if (iC > iB) {

3203
      /* '<S326>:265:1' */

3204
      tC = iB;

3205
    } else {

3206
      /* '<S326>:264:1' */

3207
      tmp = iC + 1LL;

3208
      if (tmp > 2147483647LL) {

3209
        tmp = 2147483647LL;

3210
      } else {

3211
        if (tmp < -2147483648LL) {

3212
          tmp = -2147483648LL;

3213
        }

3214
      }

3215


3216
      tC = (int32_T)tmp;

3217
    }

3218
  }

3219


3220
  return tC;

3221
}

3222


3223
/* Function for Chart: '<S316>/Landing2' */

3224
static real32_T AFCS_MODEL1_res180_m(real32_T angle)

3225
{

3226
  real32_T outangle;

3227


3228
  /* MATLAB Function 'res180': '<S326>:175' */

3229
  /* Graphical Function 'res180': '<S326>:175' */

3230
  /* '<S326>:184:1' */

3231
  if (angle > 180.0F) {

3232
    /* '<S326>:184:1' */

3233
    outangle = angle - 360.0F;

3234
  } else {

3235
    /* '<S326>:186:1' */

3236
    if (angle < -180.0F) {

3237
      /* '<S326>:186:1' */

3238
      outangle = angle + 360.0F;

3239
    } else {

3240
      /* '<S326>:191:1' */

3241
      outangle = angle;

3242
    }

3243
  }

3244


3245
  return outangle;

3246
}

3247


3248
/* Function for Chart: '<S316>/Landing2' */

3249
static void AFCS_MODEL1_Pre_Land(SimStruct *S)

3250
{

3251
  B_AFCS_MODEL1_T *_rtB;

3252
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

3253


3254
  /* During 'Pre_Land': '<S326>:357' */

3255
  if (((int32_T *)ssGetDWork(S, 301))[0] == 5) {

3256
    /* Transition: '<S326>:358' */

3257
    /* Exit Internal 'Pre_Land': '<S326>:357' */

3258
    /* Exit Internal 'XYPositionControl': '<S326>:367' */

3259
    ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3260


3261
    /* Exit Internal 'YrwControl1': '<S326>:369' */

3262
    ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3263
    ((uint8_T *)ssGetDWork(S, 537))[0] = 0U;

3264
    ((uint8_T *)ssGetDWork(S, 535))[0] = 0U;

3265
    ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Decline;

3266


3267
    /* Entry 'Decline': '<S326>:352' */

3268
    ((int32_T *)ssGetDWork(S, 302))[0] = 0;

3269
  } else if ((_rtB->GoaroundFg == 1) && (_rtB->mh_j > 30.0F) && (((int32_T *)

3270
               ssGetDWork(S, 308))[0] == 0)) {

3271
    /* Transition: '<S326>:402' */

3272
    /* Exit Internal 'Pre_Land': '<S326>:357' */

3273
    /* Exit Internal 'XYPositionControl': '<S326>:367' */

3274
    ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3275


3276
    /* Exit Internal 'YrwControl1': '<S326>:369' */

3277
    ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3278
    ((uint8_T *)ssGetDWork(S, 537))[0] = 0U;

3279
    ((uint8_T *)ssGetDWork(S, 535))[0] = 0U;

3280
    ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_GoAround;

3281


3282
    /* Entry Internal 'GoAround': '<S326>:386' */

3283
    /* Transition: '<S326>:388' */

3284
    ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_Climb;

3285


3286
    /* Entry 'Climb': '<S326>:390' */

3287
    _rtB->apthL_e = 3;

3288
    _rtB->aplonL_f = 4;

3289
    _rtB->aplatL_l = 2;

3290
    _rtB->apyawL_m = 1;

3291
    ((int32_T *)ssGetDWork(S, 307))[0] = 0;

3292
  } else {

3293
    _rtB->aplonL_f = 4;

3294
    _rtB->hcmd_j = _rtB->HrefAL;

3295
    _rtB->gamacmd_k = 0.0F;

3296
    _rtB->apthL_e = 3;

3297


3298
    /* Constant: '<S17>/Constant6' */

3299
    _rtB->vtcmd_f = 40.0F;

3300
    ((int32_T *)ssGetDWork(S, 301))[0] = AFCS_MODEL1_addup_jz((_rtB->HrefAL >

3301
      ((real32_T *)ssGetDWork(S, 286))[0]) && ((real32_T)fabs(_rtB->mh_j -

3302
      _rtB->HrefAL) < 40.0F) && ((real32_T)fabs(_rtB->yrw) < 100.0F) &&

3303
      (_rtB->GPSFail == 0), 5, ((int32_T *)ssGetDWork(S, 301))[0]);

3304


3305
    /* During 'AltitudeControl': '<S326>:359' */

3306
    /* Transition: '<S326>:361' */

3307
    if (_rtB->hcmd_j - _rtB->mh_j < -40.0F) {

3308
      /* Transition: '<S326>:364' */

3309
      _rtB->aplonL_f = 3;

3310


3311
      /* Constant: '<S17>/Constant16' */

3312
      _rtB->gamacmd_k = -5.0F;

3313
    } else if (_rtB->hcmd_j - _rtB->mh_j > 40.0F) {

3314
      /* Transition: '<S326>:363' */

3315
      _rtB->aplonL_f = 3;

3316


3317
      /* Constant: '<S17>/Constant15' */

3318
      _rtB->gamacmd_k = 5.0F;

3319
    } else {

3320
      /* Transition: '<S326>:362' */

3321
      _rtB->aplonL_f = 4;

3322
      _rtB->gamacmd_k = 0.0F;

3323
    }

3324


3325
    /* During 'XYPositionControl': '<S326>:367' */

3326
    if (((uint8_T *)ssGetDWork(S, 536))[0] == AFCS_MODEL_IN_GPSFailPhiControl) {

3327
      /* During 'GPSFailPhiControl': '<S326>:523' */

3328
      if (_rtB->GPSFail == 0) {

3329
        /* Transition: '<S326>:521' */

3330
        ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;

3331


3332
        /* Entry Internal 'YrwControl1': '<S326>:369' */

3333
        /* Transition: '<S326>:370' */

3334
        ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;

3335


3336
        /* Entry 'phiLcontrol': '<S326>:372' */

3337
        _rtB->aplatL_l = 5;

3338
        _rtB->psdcmd_i = 0.0F;

3339
        ((int32_T *)ssGetDWork(S, 305))[0] = 0;

3340
      }

3341
    } else {

3342
      /* During 'YrwControl1': '<S326>:369' */

3343
      if (_rtB->GPSFail == 1) {

3344
        /* Transition: '<S326>:522' */

3345
        /* Exit Internal 'YrwControl1': '<S326>:369' */

3346
        ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3347
        ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL_IN_GPSFailPhiControl;

3348


3349
        /* Entry 'GPSFailPhiControl': '<S326>:523' */

3350
        _rtB->aplatL_l = 2;

3351
        _rtB->apyawL_m = 1;

3352
        _rtB->phiLcmd_c = 30.0F;

3353
      } else if (((uint8_T *)ssGetDWork(S, 538))[0] ==

3354
                 AFCS_MODEL1_IN_YrwControl_o) {

3355
        /* During 'YrwControl': '<S326>:378' */

3356
        /* Transition: '<S326>:380' */

3357
        if (_rtB->yrw - _rtB->yrwcmd_o > 30.0F) {

3358
          /* Transition: '<S326>:382' */

3359
          _rtB->aplatL_l = 4;

3360
          _rtB->psdcmd_i = -20.0F;

3361
          _rtB->phiLcmd_c = 0.0F;

3362
        } else if (_rtB->yrw - _rtB->yrwcmd_o < -30.0F) {

3363
          /* Transition: '<S326>:383' */

3364
          _rtB->aplatL_l = 4;

3365
          _rtB->psdcmd_i = 20.0F;

3366
          _rtB->phiLcmd_c = 0.0F;

3367
        } else {

3368
          /* Transition: '<S326>:381' */

3369
          _rtB->aplatL_l = 5;

3370
          _rtB->yrwcmd_o = 0.0F;

3371
          _rtB->psdcmd_i = 0.0F;

3372
          _rtB->phiLcmd_c = 0.0F;

3373
        }

3374
      } else {

3375
        /* During 'phiLcontrol': '<S326>:372' */

3376
        if ((((int32_T *)ssGetDWork(S, 305))[0] == 5) && (_rtB->SWAUTO == 1)) {

3377
          /* Transition: '<S326>:371' */

3378
          ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_YrwControl_o;

3379
        } else {

3380
          ((int32_T *)ssGetDWork(S, 305))[0] = AFCS_MODEL1_addup_jz((real32_T)

3381
            fabs(AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd)) < 15.0F, 5,

3382
            ((int32_T *)ssGetDWork(S, 305))[0]);

3383


3384
          /* Transition: '<S326>:374' */

3385
          if (AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd) > 20.0F) {

3386
            /* Transition: '<S326>:375' */

3387
            _rtB->aplatL_l = 2;

3388
            _rtB->phiLcmd_c = 30.0F;

3389
          } else {

3390
            if (AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd) < -20.0F) {

3391
              /* Transition: '<S326>:377' */

3392
              _rtB->aplatL_l = 2;

3393
              _rtB->phiLcmd_c = -30.0F;

3394
            } else {

3395
              /* Transition: '<S326>:376' */

3396
              _rtB->aplatL_l = 4;

3397
              _rtB->psdcmd_i = 0.0F;

3398
              _rtB->phiLcmd_c = 0.0F;

3399
            }

3400
          }

3401
        }

3402
      }

3403
    }

3404
  }

3405
}

3406


3407
/* Function for Chart: '<S316>/Landing2' */

3408
static void AFCS_MODE_exit_internal_Landing(SimStruct *S)

3409
{

3410
  /* Exit Internal 'Landing': '<S326>:38' */

3411
  ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3412


3413
  /* Exit Internal 'GoAround': '<S326>:386' */

3414
  ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3415


3416
  /* Exit Internal 'FlightMode': '<S326>:454' */

3417
  /* Exit Internal 'XYPositionControl': '<S326>:461' */

3418
  /* Exit Internal 'EmYrwControl': '<S326>:480' */

3419
  ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3420


3421
  /* Exit Internal 'YrwControl11': '<S326>:548' */

3422
  ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3423
  ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3424


3425
  /* Exit Internal 'NormYrwControl': '<S326>:465' */

3426
  ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3427


3428
  /* Exit Internal 'YrwControl1': '<S326>:527' */

3429
  ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3430
  ((uint8_T *)ssGetDWork(S, 542))[0] = 0U;

3431
  ((uint8_T *)ssGetDWork(S, 540))[0] = 0U;

3432


3433
  /* Exit Internal 'Pre_Land': '<S326>:357' */

3434
  /* Exit Internal 'XYPositionControl': '<S326>:367' */

3435
  ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3436


3437
  /* Exit Internal 'YrwControl1': '<S326>:369' */

3438
  ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3439
  ((uint8_T *)ssGetDWork(S, 537))[0] = 0U;

3440
  ((uint8_T *)ssGetDWork(S, 535))[0] = 0U;

3441
}

3442


3443
/* Function for Chart: '<S316>/Landing2' */

3444
static void AFCS_MODEL1_XYPositionControl_d(SimStruct *S)

3445
{

3446
  int64_T tmp;

3447
  B_AFCS_MODEL1_T *_rtB;

3448
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

3449


3450
  /* During 'XYPositionControl': '<S326>:461' */

3451
  if (((uint8_T *)ssGetDWork(S, 541))[0] == AFCS_MODEL1_IN_EmYrwControl) {

3452
    /* During 'EmYrwControl': '<S326>:480' */

3453
    if (((int8_T *)ssGetDWork(S, 511))[0] == 1) {

3454
      /* Transition: '<S326>:463' */

3455
      /* Exit Internal 'EmYrwControl': '<S326>:480' */

3456
      ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3457


3458
      /* Exit Internal 'YrwControl11': '<S326>:548' */

3459
      ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3460
      ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NormYrwControl;

3461


3462
      /* Entry 'NormYrwControl': '<S326>:465' */

3463
      /* Entry Internal 'NormYrwControl': '<S326>:465' */

3464
      /* Transition: '<S326>:547' */

3465
      ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_YrwControl1;

3466


3467
      /* Entry Internal 'YrwControl1': '<S326>:527' */

3468
      /* Transition: '<S326>:545' */

3469
      ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_phiLcontrol;

3470


3471
      /* Entry 'phiLcontrol': '<S326>:533' */

3472
      _rtB->aplatL_l = 4;

3473
      _rtB->apyawL_m = 1;

3474
      _rtB->psdcmd_i = 0.0F;

3475
      ((int32_T *)ssGetDWork(S, 305))[0] = 0;

3476
    } else {

3477
      switch (((uint8_T *)ssGetDWork(S, 545))[0]) {

3478
       case AFCS_MODEL_IN_GPSFailPhiControl:

3479
        /* During 'GPSFailPhiControl': '<S326>:569' */

3480
        if (_rtB->GPSFail == 0) {

3481
          /* Transition: '<S326>:571' */

3482
          ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_YrwControl11;

3483


3484
          /* Entry Internal 'YrwControl11': '<S326>:548' */

3485
          /* Transition: '<S326>:561' */

3486
          ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_phiLcontrol2;

3487


3488
          /* Entry 'phiLcontrol2': '<S326>:555' */

3489
          _rtB->aplatL_l = 2;

3490
          _rtB->apyawL_m = 1;

3491
          ((int32_T *)ssGetDWork(S, 309))[0] = 0;

3492
          ((int8_T *)ssGetDWork(S, 511))[0] = 0;

3493
        } else {

3494
          _rtB->aplatL_l = 2;

3495
          _rtB->apyawL_m = 1;

3496
          _rtB->phiLcmd_c = 30.0F;

3497
          ((int32_T *)ssGetDWork(S, 310))[0] = 0;

3498
        }

3499
        break;

3500


3501
       case AFCS_MODEL1_IN_PhiLontrol3:

3502
        /* During 'PhiLontrol3': '<S326>:497' */

3503
        if (((int32_T *)ssGetDWork(S, 312))[0] == 5) {

3504
          /* Transition: '<S326>:484' */

3505
          ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_TranseNorm_c;

3506


3507
          /* Entry 'TranseNorm': '<S326>:498' */

3508
          _rtB->StartEmNav_a = 0;

3509
          ((int8_T *)ssGetDWork(S, 511))[0] = 1;

3510
          tmp = ((int32_T *)ssGetDWork(S, 308))[0] + 1LL;

3511
          if (tmp > 2147483647LL) {

3512
            tmp = 2147483647LL;

3513
          } else {

3514
            if (tmp < -2147483648LL) {

3515
              tmp = -2147483648LL;

3516
            }

3517
          }

3518


3519
          ((int32_T *)ssGetDWork(S, 308))[0] = (int32_T)tmp;

3520
        } else {

3521
          _rtB->phiLcmd_c = 30.0F * _rtB->DireFg;

3522
          ((int32_T *)ssGetDWork(S, 312))[0] = AFCS_MODEL1_addup_jz((real32_T)

3523
            fabs(AFCS_MODEL1_res180_m(_rtB->DetaPsi)) < 15.0F, 5, ((int32_T *)

3524
            ssGetDWork(S, 312))[0]);

3525
        }

3526
        break;

3527


3528
       case AFCS_MODEL1_IN_TranseNorm_c:

3529
        /* During 'TranseNorm': '<S326>:498' */

3530
        break;

3531


3532
       default:

3533
        /* During 'YrwControl11': '<S326>:548' */

3534
        if (_rtB->GPSFail == 1) {

3535
          /* Transition: '<S326>:570' */

3536
          /* Exit Internal 'YrwControl11': '<S326>:548' */

3537
          ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3538
          ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL_IN_GPSFailPhiControl;

3539
        } else if (((int32_T *)ssGetDWork(S, 310))[0] == 5) {

3540
          /* Transition: '<S326>:483' */

3541
          /* Exit Internal 'YrwControl11': '<S326>:548' */

3542
          ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3543
          ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_PhiLontrol3;

3544


3545
          /* Entry 'PhiLontrol3': '<S326>:497' */

3546
          _rtB->aplatL_l = 2;

3547
          _rtB->apyawL_m = 1;

3548
          ((int32_T *)ssGetDWork(S, 312))[0] = 0;

3549
        } else if (((uint8_T *)ssGetDWork(S, 546))[0] ==

3550
                   AFCS_MODEL1_IN_YrwControl_o) {

3551
          /* During 'YrwControl': '<S326>:563' */

3552
          ((int32_T *)ssGetDWork(S, 310))[0] = AFCS_MODEL1_addup_jz

3553
            ((_rtB->drange < 50.0F) && (_rtB->GPSFail == 0), 5, ((int32_T *)

3554
              ssGetDWork(S, 310))[0]);

3555


3556
          /* Transition: '<S326>:565' */

3557
          if (_rtB->yrw - _rtB->yrwcmd_o > 50.0F) {

3558
            /* Transition: '<S326>:567' */

3559
            _rtB->aplatL_l = 4;

3560
            _rtB->psdcmd_i = -10.0F;

3561
            _rtB->phiLcmd_c = 0.0F;

3562
          } else if (_rtB->yrw - _rtB->yrwcmd_o < -50.0F) {

3563
            /* Transition: '<S326>:568' */

3564
            _rtB->aplatL_l = 4;

3565
            _rtB->psdcmd_i = 10.0F;

3566
            _rtB->phiLcmd_c = 0.0F;

3567
          } else {

3568
            /* Transition: '<S326>:566' */

3569
            _rtB->aplatL_l = 5;

3570
            _rtB->yrwcmd_o = 0.0F;

3571
            _rtB->psdcmd_i = 0.0F;

3572
            _rtB->phiLcmd_c = 0.0F;

3573
          }

3574
        } else {

3575
          /* During 'phiLcontrol2': '<S326>:555' */

3576
          if ((((int32_T *)ssGetDWork(S, 309))[0] == 5) && (_rtB->SWAUTO == 1))

3577
          {

3578
            /* Transition: '<S326>:562' */

3579
            _rtB->StartEmNav_a = 1;

3580
            ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_YrwControl_o;

3581


3582
            /* Entry 'YrwControl': '<S326>:563' */

3583
            ((int32_T *)ssGetDWork(S, 310))[0] = 0;

3584
          } else {

3585
            ((int32_T *)ssGetDWork(S, 309))[0] = AFCS_MODEL1_addup_jz((real32_T)

3586
              fabs(AFCS_MODEL1_res180_m(_rtB->DetaPsi)) < 15.0F, 5, ((int32_T *)

3587
              ssGetDWork(S, 309))[0]);

3588


3589
            /* Transition: '<S326>:557' */

3590
            if (AFCS_MODEL1_res180_m(_rtB->DetaPsi) > 15.0F) {

3591
              /* Transition: '<S326>:558' */

3592
              _rtB->aplatL_l = 2;

3593
              _rtB->phiLcmd_c = 30.0F;

3594
            } else {

3595
              if (AFCS_MODEL1_res180_m(_rtB->DetaPsi) < -15.0F) {

3596
                /* Transition: '<S326>:560' */

3597
                _rtB->aplatL_l = 2;

3598
                _rtB->phiLcmd_c = -30.0F;

3599
              } else {

3600
                /* Transition: '<S326>:559' */

3601
                _rtB->aplatL_l = 2;

3602
                _rtB->phiLcmd_c = 0.0F;

3603
              }

3604
            }

3605
          }

3606
        }

3607
        break;

3608
      }

3609
    }

3610
  } else {

3611
    /* During 'NormYrwControl': '<S326>:465' */

3612
    if ((_rtB->GoaroundFg == 1) && (((int32_T *)ssGetDWork(S, 308))[0] == 0)) {

3613
      /* Transition: '<S326>:464' */

3614
      /* Exit Internal 'NormYrwControl': '<S326>:465' */

3615
      ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3616


3617
      /* Exit Internal 'YrwControl1': '<S326>:527' */

3618
      ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3619
      ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_EmYrwControl;

3620


3621
      /* Entry Internal 'EmYrwControl': '<S326>:480' */

3622
      /* Transition: '<S326>:481' */

3623
      ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_YrwControl11;

3624


3625
      /* Entry Internal 'YrwControl11': '<S326>:548' */

3626
      /* Transition: '<S326>:561' */

3627
      ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_phiLcontrol2;

3628


3629
      /* Entry 'phiLcontrol2': '<S326>:555' */

3630
      _rtB->aplatL_l = 2;

3631
      _rtB->apyawL_m = 1;

3632
      ((int32_T *)ssGetDWork(S, 309))[0] = 0;

3633
      ((int8_T *)ssGetDWork(S, 511))[0] = 0;

3634
    } else if (((uint8_T *)ssGetDWork(S, 543))[0] ==

3635
               AFCS_MODEL_IN_GPSFailPhiControl) {

3636
      /* During 'GPSFailPhiControl': '<S326>:526' */

3637
      if (_rtB->GPSFail == 0) {

3638
        /* Transition: '<S326>:524' */

3639
        ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_YrwControl1;

3640


3641
        /* Entry Internal 'YrwControl1': '<S326>:527' */

3642
        /* Transition: '<S326>:545' */

3643
        ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_phiLcontrol;

3644


3645
        /* Entry 'phiLcontrol': '<S326>:533' */

3646
        _rtB->aplatL_l = 4;

3647
        _rtB->apyawL_m = 1;

3648
        _rtB->psdcmd_i = 0.0F;

3649
        ((int32_T *)ssGetDWork(S, 305))[0] = 0;

3650
      }

3651
    } else {

3652
      /* During 'YrwControl1': '<S326>:527' */

3653
      if (_rtB->GPSFail == 1) {

3654
        /* Transition: '<S326>:525' */

3655
        /* Exit Internal 'YrwControl1': '<S326>:527' */

3656
        ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3657
        ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL_IN_GPSFailPhiControl;

3658


3659
        /* Entry 'GPSFailPhiControl': '<S326>:526' */

3660
        _rtB->aplatL_l = 2;

3661
        _rtB->apyawL_m = 1;

3662
        _rtB->phiLcmd_c = 30.0F;

3663
      } else if (((uint8_T *)ssGetDWork(S, 544))[0] ==

3664
                 AFCS_MODEL1_IN_YrwControl_o) {

3665
        /* During 'YrwControl': '<S326>:539' */

3666
        /* Transition: '<S326>:541' */

3667
        if (_rtB->yrw - _rtB->yrwcmd_o > 50.0F) {

3668
          /* Transition: '<S326>:543' */

3669
          _rtB->aplatL_l = 4;

3670
          _rtB->psdcmd_i = -10.0F;

3671
          _rtB->phiLcmd_c = 0.0F;

3672
        } else if (_rtB->yrw - _rtB->yrwcmd_o < -50.0F) {

3673
          /* Transition: '<S326>:544' */

3674
          _rtB->aplatL_l = 4;

3675
          _rtB->psdcmd_i = 10.0F;

3676
          _rtB->phiLcmd_c = 0.0F;

3677
        } else {

3678
          /* Transition: '<S326>:542' */

3679
          _rtB->aplatL_l = 5;

3680
          _rtB->yrwcmd_o = 0.0F;

3681
          _rtB->psdcmd_i = 0.0F;

3682
          _rtB->phiLcmd_c = 0.0F;

3683
        }

3684
      } else {

3685
        /* During 'phiLcontrol': '<S326>:533' */

3686
        if ((((int32_T *)ssGetDWork(S, 305))[0] == 5) && (_rtB->SWAUTO == 1) &&

3687
            (_rtB->GPSFail == 0)) {

3688
          /* Transition: '<S326>:546' */

3689
          ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_YrwControl_o;

3690
        } else {

3691
          ((int32_T *)ssGetDWork(S, 305))[0] = AFCS_MODEL1_addup_jz((real32_T)

3692
            fabs(AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd)) < 15.0F, 5,

3693
            ((int32_T *)ssGetDWork(S, 305))[0]);

3694


3695
          /* Transition: '<S326>:535' */

3696
          if (AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd) > 20.0F) {

3697
            /* Transition: '<S326>:536' */

3698
            _rtB->aplatL_l = 2;

3699
            _rtB->phiLcmd_c = 30.0F;

3700
          } else {

3701
            if (AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd) < -20.0F) {

3702
              /* Transition: '<S326>:538' */

3703
              _rtB->aplatL_l = 2;

3704
              _rtB->phiLcmd_c = -30.0F;

3705
            } else {

3706
              /* Transition: '<S326>:537' */

3707
              _rtB->aplatL_l = 4;

3708
              _rtB->psdcmd_i = 0.0F;

3709
              _rtB->phiLcmd_c = 0.0F;

3710
            }

3711
          }

3712
        }

3713
      }

3714
    }

3715
  }

3716
}

3717


3718
/* Function for Chart: '<S316>/Landing2' */

3719
static void AFCS_MODEL1_Landing(SimStruct *S)

3720
{

3721
  int32_T tmp;

3722
  B_AFCS_MODEL1_T *_rtB;

3723
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

3724


3725
  /* Constant: '<S17>/Constant11' */

3726
  /* During 'Landing': '<S326>:38' */

3727
  ((real32_T *)ssGetDWork(S, 286))[0] = _rtB->xrw * 0.0349181965F;

3728
  if (_rtB->Switch6_j == 1) {

3729
    /* Transition: '<S326>:520' */

3730
    _rtB->fmLandtoTaxi = 1;

3731
    tmp = _rtB->K_k + 1;

3732
    if (tmp > 127) {

3733
      tmp = 127;

3734
    }

3735


3736
    _rtB->K_k = (int8_T)tmp;

3737
    AFCS_MODE_exit_internal_Landing(S);

3738


3739
    /* Entry Internal 'Landing': '<S326>:38' */

3740
    /* Transition: '<S326>:347' */

3741
    ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Pre_Land;

3742


3743
    /* Entry 'Pre_Land': '<S326>:357' */

3744
    ((int32_T *)ssGetDWork(S, 301))[0] = 0;

3745


3746
    /* Entry Internal 'Pre_Land': '<S326>:357' */

3747
    ((uint8_T *)ssGetDWork(S, 535))[0] = 1U;

3748
    ((uint8_T *)ssGetDWork(S, 537))[0] = 1U;

3749


3750
    /* Entry Internal 'XYPositionControl': '<S326>:367' */

3751
    /* Transition: '<S326>:368' */

3752
    ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;

3753


3754
    /* Entry Internal 'YrwControl1': '<S326>:369' */

3755
    /* Transition: '<S326>:370' */

3756
    ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;

3757


3758
    /* Entry 'phiLcontrol': '<S326>:372' */

3759
    _rtB->aplatL_l = 5;

3760
    _rtB->psdcmd_i = 0.0F;

3761
    ((int32_T *)ssGetDWork(S, 305))[0] = 0;

3762
  } else {

3763
    switch (((uint8_T *)ssGetDWork(S, 534))[0]) {

3764
     case AFCS_MODEL1_IN_Decline:

3765
      /* During 'Decline': '<S326>:352' */

3766
      if (((int32_T *)ssGetDWork(S, 302))[0] == 5) {

3767
        /* Transition: '<S326>:354' */

3768
        ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_GamaControl1;

3769


3770
        /* Entry 'GamaControl1': '<S326>:202' */

3771
        _rtB->aplonL_f = 3;

3772
        _rtB->gamacmd_k = -1.5F;

3773
        _rtB->apthL_e = 3;

3774
        _rtB->thrustcmd_ho = 0.0F;

3775
        _rtB->vtcmd_f = 30.0F;

3776
        _rtB->aplatL_l = 5;

3777
        ((int32_T *)ssGetDWork(S, 303))[0] = 0;

3778
      } else if ((_rtB->GoaroundFg == 1) && (_rtB->mh_j > 30.0F) && (((int32_T *)

3779
                   ssGetDWork(S, 308))[0] == 0)) {

3780
        /* Transition: '<S326>:387' */

3781
        ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_GoAround;

3782


3783
        /* Entry Internal 'GoAround': '<S326>:386' */

3784
        /* Transition: '<S326>:388' */

3785
        ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_Climb;

3786


3787
        /* Entry 'Climb': '<S326>:390' */

3788
        _rtB->apthL_e = 3;

3789
        _rtB->aplonL_f = 4;

3790
        _rtB->aplatL_l = 2;

3791
        _rtB->apyawL_m = 1;

3792
        ((int32_T *)ssGetDWork(S, 307))[0] = 0;

3793
      } else if ((_rtB->GPSFail == 1) && (_rtB->mh_j > 15.0F)) {

3794
        /* Transition: '<S326>:575' */

3795
        ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Pre_Land;

3796


3797
        /* Entry 'Pre_Land': '<S326>:357' */

3798
        ((int32_T *)ssGetDWork(S, 301))[0] = 0;

3799


3800
        /* Entry Internal 'Pre_Land': '<S326>:357' */

3801
        ((uint8_T *)ssGetDWork(S, 535))[0] = 1U;

3802
        ((uint8_T *)ssGetDWork(S, 537))[0] = 1U;

3803


3804
        /* Entry Internal 'XYPositionControl': '<S326>:367' */

3805
        /* Transition: '<S326>:368' */

3806
        ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;

3807


3808
        /* Entry Internal 'YrwControl1': '<S326>:369' */

3809
        /* Transition: '<S326>:370' */

3810
        ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;

3811


3812
        /* Entry 'phiLcontrol': '<S326>:372' */

3813
        _rtB->aplatL_l = 5;

3814
        _rtB->psdcmd_i = 0.0F;

3815
        ((int32_T *)ssGetDWork(S, 305))[0] = 0;

3816
      } else {

3817
        _rtB->aplonL_f = 4;

3818
        _rtB->hcmd_j = ((real32_T *)ssGetDWork(S, 286))[0];

3819
        _rtB->apsbL_l = 2;

3820
        _rtB->aplatL_l = 5;

3821
        _rtB->psdcmd_i = 0.0F;

3822
        _rtB->phiLcmd_c = 0.0F;

3823
        _rtB->apnwsL_m = 2;

3824


3825
        /* Constant: '<S17>/Constant11' */

3826
        _rtB->gamacmd_k = -2.0F;

3827
        _rtB->apthL_e = 3;

3828


3829
        /* Constant: '<S17>/Constant4' */

3830
        _rtB->vtcmd_f = 35.0F;

3831
        ((int32_T *)ssGetDWork(S, 302))[0] = AFCS_MODEL1_addup_jz(_rtB->mh_j <

3832
          10.0F, 5, ((int32_T *)ssGetDWork(S, 302))[0]);

3833
        if ((_rtB->GPSFail == 1) && (_rtB->mh_j < 15.0F)) {

3834
          /* Transition: '<S326>:574' */

3835
          _rtB->aplatL_l = 1;

3836
          _rtB->phiLcmd_c = 0.0F;

3837
        }

3838
      }

3839
      break;

3840


3841
     case AFCS_MODEL1_IN_GamaControl1:

3842
      /* During 'GamaControl1': '<S326>:202' */

3843
      if (((int32_T *)ssGetDWork(S, 303))[0] == 5) {

3844
        /* Transition: '<S326>:205' */

3845
        ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_GamaControl2;

3846


3847
        /* Entry 'GamaControl2': '<S326>:204' */

3848
        _rtB->aplatL_l = 2;

3849
        _rtB->phiLcmd_c = 0.0F;

3850
        _rtB->gamacmd_k = -0.5F;

3851
        _rtB->apyawL_m = 3;

3852
        _rtB->apthL_e = 3;

3853
        _rtB->vtcmd_f = 30.0F;

3854
        _rtB->thrustcmd_ho = 0.0F;

3855
        ((int32_T *)ssGetDWork(S, 304))[0] = 0;

3856
      } else {

3857
        ((int32_T *)ssGetDWork(S, 303))[0] = AFCS_MODEL1_addup_jz(_rtB->mh_j <

3858
          4.0F, 5, ((int32_T *)ssGetDWork(S, 303))[0]);

3859
      }

3860
      break;

3861


3862
     case AFCS_MODEL1_IN_GamaControl2:

3863
      /* During 'GamaControl2': '<S326>:204' */

3864
      if (((int32_T *)ssGetDWork(S, 304))[0] == 5) {

3865
        /* Transition: '<S326>:207' */

3866
        ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_VupControl3;

3867


3868
        /* Entry 'VupControl3': '<S326>:206' */

3869
        _rtB->fmLandtoTaxi = 1;

3870
      } else {

3871
        ((int32_T *)ssGetDWork(S, 304))[0] = AFCS_MODEL1_addup_jz((_rtB->mh_j <

3872
          1.0F) && (_rtB->Switch6_j == 1), 5, ((int32_T *)ssGetDWork(S, 304))[0]);

3873
      }

3874
      break;

3875


3876
     case AFCS_MODEL1_IN_GoAround:

3877
      /* During 'GoAround': '<S326>:386' */

3878
      if (((int8_T *)ssGetDWork(S, 512))[0] == 1) {

3879
        /* Transition: '<S326>:501' */

3880
        /* Exit Internal 'GoAround': '<S326>:386' */

3881
        ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3882


3883
        /* Exit Internal 'FlightMode': '<S326>:454' */

3884
        /* Exit Internal 'XYPositionControl': '<S326>:461' */

3885
        /* Exit Internal 'EmYrwControl': '<S326>:480' */

3886
        ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3887


3888
        /* Exit Internal 'YrwControl11': '<S326>:548' */

3889
        ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3890
        ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3891


3892
        /* Exit Internal 'NormYrwControl': '<S326>:465' */

3893
        ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3894


3895
        /* Exit Internal 'YrwControl1': '<S326>:527' */

3896
        ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3897
        ((uint8_T *)ssGetDWork(S, 542))[0] = 0U;

3898
        ((uint8_T *)ssGetDWork(S, 540))[0] = 0U;

3899
        ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Pre_Land;

3900


3901
        /* Entry 'Pre_Land': '<S326>:357' */

3902
        ((int32_T *)ssGetDWork(S, 301))[0] = 0;

3903


3904
        /* Entry Internal 'Pre_Land': '<S326>:357' */

3905
        ((uint8_T *)ssGetDWork(S, 535))[0] = 1U;

3906
        ((uint8_T *)ssGetDWork(S, 537))[0] = 1U;

3907


3908
        /* Entry Internal 'XYPositionControl': '<S326>:367' */

3909
        /* Transition: '<S326>:368' */

3910
        ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;

3911


3912
        /* Entry Internal 'YrwControl1': '<S326>:369' */

3913
        /* Transition: '<S326>:370' */

3914
        ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;

3915


3916
        /* Entry 'phiLcontrol': '<S326>:372' */

3917
        _rtB->aplatL_l = 5;

3918
        _rtB->psdcmd_i = 0.0F;

3919
        ((int32_T *)ssGetDWork(S, 305))[0] = 0;

3920
      } else {

3921
        switch (((uint8_T *)ssGetDWork(S, 539))[0]) {

3922
         case AFCS_MODEL1_IN_Climb:

3923
          /* During 'Climb': '<S326>:390' */

3924
          if (((int32_T *)ssGetDWork(S, 307))[0] == 5) {

3925
            /* Transition: '<S326>:399' */

3926
            ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_LeftSpiral10s;

3927


3928
            /* Entry 'LeftSpiral10s': '<S326>:398' */

3929
            _rtB->apthL_e = 3;

3930
            _rtB->aplonL_f = 4;

3931
            _rtB->aplatL_l = 2;

3932
            _rtB->apyawL_m = 1;

3933
            ((int32_T *)ssGetDWork(S, 306))[0] = 0;

3934
          } else {

3935
            _rtB->hcmd_j = _rtB->HrefAL;

3936


3937
            /* Constant: '<S17>/Constant6' */

3938
            _rtB->vtcmd_f = 40.0F;

3939
            _rtB->phiLcmd_c = 0.0F;

3940
            ((int32_T *)ssGetDWork(S, 307))[0] = AFCS_MODEL1_addup_jz(_rtB->mh_j

3941
              > 30.0F, 5, ((int32_T *)ssGetDWork(S, 307))[0]);

3942


3943
            /* Transition: '<S326>:392' */

3944
            if (_rtB->hcmd_j - _rtB->mh_j < -40.0F) {

3945
              /* Transition: '<S326>:394' */

3946
              _rtB->aplonL_f = 3;

3947


3948
              /* Constant: '<S17>/Constant16' */

3949
              _rtB->gamacmd_k = -5.0F;

3950
            } else if (_rtB->hcmd_j - _rtB->mh_j > 40.0F) {

3951
              /* Transition: '<S326>:396' */

3952
              _rtB->aplonL_f = 3;

3953


3954
              /* Constant: '<S17>/Constant15' */

3955
              _rtB->gamacmd_k = 5.0F;

3956
            } else {

3957
              /* Transition: '<S326>:393' */

3958
              _rtB->aplonL_f = 4;

3959
              _rtB->gamacmd_k = 0.0F;

3960
            }

3961
          }

3962
          break;

3963


3964
         case AFCS_MODEL1_IN_FlightMode_e:

3965
          /* During 'FlightMode': '<S326>:454' */

3966
          if (((int32_T *)ssGetDWork(S, 311))[0] == 5) {

3967
            /* Transition: '<S326>:500' */

3968
            /* Exit Internal 'FlightMode': '<S326>:454' */

3969
            /* Exit Internal 'XYPositionControl': '<S326>:461' */

3970
            /* Exit Internal 'EmYrwControl': '<S326>:480' */

3971
            ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3972


3973
            /* Exit Internal 'YrwControl11': '<S326>:548' */

3974
            ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3975
            ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3976


3977
            /* Exit Internal 'NormYrwControl': '<S326>:465' */

3978
            ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3979


3980
            /* Exit Internal 'YrwControl1': '<S326>:527' */

3981
            ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

3982
            ((uint8_T *)ssGetDWork(S, 542))[0] = 0U;

3983
            ((uint8_T *)ssGetDWork(S, 540))[0] = 0U;

3984
            ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_TransLand_o;

3985


3986
            /* Entry 'TransLand': '<S326>:499' */

3987
            ((int8_T *)ssGetDWork(S, 512))[0] = 1;

3988
            _rtB->K_k = _rtB->IndexAL;

3989
          } else {

3990
            /* Constant: '<S17>/Constant10' */

3991
            ((int32_T *)ssGetDWork(S, 311))[0] = AFCS_MODEL1_addup_jz

3992
              ((_rtB->drange < 400.0F) && ((real32_T)fabs(_rtB->href -

3993
                 _rtB->mh_j) < 50.0F) && ((real32_T)fabs(_rtB->yrw -

3994
                 _rtB->yrwcmd_o) < 50.0F) && (_rtB->SWAUTO == 1) &&

3995
               (_rtB->StartEmNav_a == 0), 5, ((int32_T *)ssGetDWork(S, 311))[0]);

3996


3997
            /* During 'AltitudeControl': '<S326>:455' */

3998
            /* Transition: '<S326>:457' */

3999
            if (_rtB->hcmd_j - _rtB->mh_j < -20.0F) {

4000
              /* Transition: '<S326>:460' */

4001
              _rtB->aplonL_f = 3;

4002


4003
              /* Constant: '<S17>/Constant16' */

4004
              _rtB->gamacmd_k = -5.0F;

4005
            } else if (_rtB->hcmd_j - _rtB->mh_j > 20.0F) {

4006
              /* Transition: '<S326>:459' */

4007
              _rtB->aplonL_f = 3;

4008


4009
              /* Constant: '<S17>/Constant15' */

4010
              _rtB->gamacmd_k = 5.0F;

4011
            } else {

4012
              /* Transition: '<S326>:458' */

4013
              _rtB->aplonL_f = 4;

4014
              _rtB->gamacmd_k = 0.0F;

4015
            }

4016


4017
            AFCS_MODEL1_XYPositionControl_d(S);

4018
          }

4019
          break;

4020


4021
         case AFCS_MODEL1_IN_LeftSpiral10s:

4022
          /* During 'LeftSpiral10s': '<S326>:398' */

4023
          if (((int32_T *)ssGetDWork(S, 306))[0] == 2500) {

4024
            /* Transition: '<S326>:401' */

4025
            ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_FlightMode_e;

4026


4027
            /* Entry 'FlightMode': '<S326>:454' */

4028
            ((int32_T *)ssGetDWork(S, 311))[0] = 0;

4029
            tmp = _rtB->IndexAL - 1;

4030
            if (tmp < -128) {

4031
              tmp = -128;

4032
            }

4033


4034
            _rtB->K_k = (int8_T)tmp;

4035


4036
            /* Entry Internal 'FlightMode': '<S326>:454' */

4037
            ((uint8_T *)ssGetDWork(S, 540))[0] = 1U;

4038
            ((uint8_T *)ssGetDWork(S, 542))[0] = 1U;

4039


4040
            /* Entry Internal 'XYPositionControl': '<S326>:461' */

4041
            /* Transition: '<S326>:462' */

4042
            ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NormYrwControl;

4043


4044
            /* Entry 'NormYrwControl': '<S326>:465' */

4045
            /* Entry Internal 'NormYrwControl': '<S326>:465' */

4046
            /* Transition: '<S326>:547' */

4047
            ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_YrwControl1;

4048


4049
            /* Entry Internal 'YrwControl1': '<S326>:527' */

4050
            /* Transition: '<S326>:545' */

4051
            ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_phiLcontrol;

4052


4053
            /* Entry 'phiLcontrol': '<S326>:533' */

4054
            _rtB->aplatL_l = 4;

4055
            _rtB->apyawL_m = 1;

4056
            _rtB->psdcmd_i = 0.0F;

4057
            ((int32_T *)ssGetDWork(S, 305))[0] = 0;

4058
          } else {

4059
            _rtB->hcmd_j = _rtB->HrefAL;

4060


4061
            /* Constant: '<S17>/Constant6' */

4062
            _rtB->vtcmd_f = 40.0F;

4063
            _rtB->phiLcmd_c = -30.0F;

4064
            ((int32_T *)ssGetDWork(S, 306))[0] = AFCS_MODEL1_addup_jz(_rtB->mh_j

4065
              > _rtB->HrefAL - 20.0F, 2500, ((int32_T *)ssGetDWork(S, 306))[0]);

4066
          }

4067
          break;

4068


4069
         default:

4070
          /* During 'TransLand': '<S326>:499' */

4071
          break;

4072
        }

4073
      }

4074
      break;

4075


4076
     case AFCS_MODEL1_IN_Pre_Land:

4077
      AFCS_MODEL1_Pre_Land(S);

4078
      break;

4079


4080
     default:

4081
      /* During 'VupControl3': '<S326>:206' */

4082
      break;

4083
    }

4084
  }

4085
}

4086


4087
/* Function for Chart: '<S322>/Taxi' */

4088
static int32_T AFCS_MODEL1_addup_od(boolean_T bA, int32_T iB, int32_T iC)

4089
{

4090
  int32_T tC;

4091
  int64_T tmp;

4092


4093
  /* MATLAB Function 'addup': '<S330>:121' */

4094
  /* Graphical Function 'addup': '<S330>:121' */

4095
  /* '<S330>:129:1' */

4096
  tC = iC;

4097


4098
  /* '<S330>:130:1' */

4099
  if (!bA) {

4100
    /* '<S330>:133:1' */

4101
    if (iC > 0) {

4102
      /* '<S330>:133:1' */

4103
      tC = iC - 1;

4104
    }

4105
  } else {

4106
    /* '<S330>:135:1' */

4107
    if (iC > iB) {

4108
      /* '<S330>:135:1' */

4109
      tC = iB;

4110
    } else {

4111
      /* '<S330>:134:1' */

4112
      tmp = iC + 1LL;

4113
      if (tmp > 2147483647LL) {

4114
        tmp = 2147483647LL;

4115
      } else {

4116
        if (tmp < -2147483648LL) {

4117
          tmp = -2147483648LL;

4118
        }

4119
      }

4120


4121
      tC = (int32_T)tmp;

4122
    }

4123
  }

4124


4125
  return tC;

4126
}

4127


4128
/* Function for Chart: '<S319>/Pre_Takeoff' */

4129
static int32_T AFCS_MODEL1_addup_ke(int8_T bA, int32_T iB, int32_T iC)

4130
{

4131
  int32_T tC;

4132
  int64_T tmp;

4133


4134
  /* MATLAB Function 'addup': '<S327>:8' */

4135
  /* Graphical Function 'addup': '<S327>:8' */

4136
  /* '<S327>:13:1' */

4137
  tC = iC;

4138


4139
  /* '<S327>:16:1' */

4140
  if (bA == 0) {

4141
    /* '<S327>:18:1' */

4142
    if (iC > 0) {

4143
      /* '<S327>:18:1' */

4144
      tC = iC - 1;

4145
    }

4146
  } else {

4147
    /* '<S327>:24:1' */

4148
    if (iC > iB) {

4149
      /* '<S327>:24:1' */

4150
      tC = iB;

4151
    } else {

4152
      /* '<S327>:26:1' */

4153
      tmp = iC + 1LL;

4154
      if (tmp > 2147483647LL) {

4155
        tmp = 2147483647LL;

4156
      } else {

4157
        if (tmp < -2147483648LL) {

4158
          tmp = -2147483648LL;

4159
        }

4160
      }

4161


4162
      tC = (int32_T)tmp;

4163
    }

4164
  }

4165


4166
  return tC;

4167
}

4168


4169
/* Function for Chart: '<S314>/Climb2' */

4170
static int32_T AFCS_MODEL1_addup_j(boolean_T bA, int32_T iB, int32_T iC)

4171
{

4172
  int32_T tC;

4173
  int64_T tmp;

4174


4175
  /* MATLAB Function 'addup': '<S324>:154' */

4176
  /* Graphical Function 'addup': '<S324>:154' */

4177
  /* '<S324>:162:1' */

4178
  tC = iC;

4179


4180
  /* '<S324>:163:1' */

4181
  if (!bA) {

4182
    /* '<S324>:166:1' */

4183
    if (iC > 0) {

4184
      /* '<S324>:166:1' */

4185
      tC = iC - 1;

4186
    }

4187
  } else {

4188
    /* '<S324>:168:1' */

4189
    if (iC > iB) {

4190
      /* '<S324>:168:1' */

4191
      tC = iB;

4192
    } else {

4193
      /* '<S324>:167:1' */

4194
      tmp = iC + 1LL;

4195
      if (tmp > 2147483647LL) {

4196
        tmp = 2147483647LL;

4197
      } else {

4198
        if (tmp < -2147483648LL) {

4199
          tmp = -2147483648LL;

4200
        }

4201
      }

4202


4203
      tC = (int32_T)tmp;

4204
    }

4205
  }

4206


4207
  return tC;

4208
}

4209


4210
/* Function for Chart: '<S313>/Approach2' */

4211
static int32_T AFCS_MODEL1_addup_f(boolean_T bA, int32_T iB, int32_T iC)

4212
{

4213
  int32_T tC;

4214
  int64_T tmp;

4215


4216
  /* MATLAB Function 'addup': '<S323>:413' */

4217
  /* Graphical Function 'addup': '<S323>:413' */

4218
  /* '<S323>:421:1' */

4219
  tC = iC;

4220


4221
  /* '<S323>:422:1' */

4222
  if (!bA) {

4223
    /* '<S323>:425:1' */

4224
    if (iC > 0) {

4225
      /* '<S323>:425:1' */

4226
      tC = iC - 1;

4227
    }

4228
  } else {

4229
    /* '<S323>:427:1' */

4230
    if (iC > iB) {

4231
      /* '<S323>:427:1' */

4232
      tC = iB;

4233
    } else {

4234
      /* '<S323>:426:1' */

4235
      tmp = iC + 1LL;

4236
      if (tmp > 2147483647LL) {

4237
        tmp = 2147483647LL;

4238
      } else {

4239
        if (tmp < -2147483648LL) {

4240
          tmp = -2147483648LL;

4241
        }

4242
      }

4243


4244
      tC = (int32_T)tmp;

4245
    }

4246
  }

4247


4248
  return tC;

4249
}

4250


4251
/* Function for Chart: '<S313>/Approach2' */

4252
static real32_T AFCS_MODEL1_res180_l(real32_T angle)

4253
{

4254
  real32_T outangle;

4255


4256
  /* MATLAB Function 'res180': '<S323>:175' */

4257
  /* Graphical Function 'res180': '<S323>:175' */

4258
  /* '<S323>:184:1' */

4259
  if (angle > 180.0F) {

4260
    /* '<S323>:184:1' */

4261
    outangle = angle - 360.0F;

4262
  } else {

4263
    /* '<S323>:186:1' */

4264
    if (angle < -180.0F) {

4265
      /* '<S323>:186:1' */

4266
      outangle = angle + 360.0F;

4267
    } else {

4268
      /* '<S323>:191:1' */

4269
      outangle = angle;

4270
    }

4271
  }

4272


4273
  return outangle;

4274
}

4275


4276
/* Function for Chart: '<S313>/Approach2' */

4277
static void AFCS_MODEL1_XYPositionControl(SimStruct *S)

4278
{

4279
  int64_T tmp;

4280
  B_AFCS_MODEL1_T *_rtB;

4281
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

4282


4283
  /* During 'XYPositionControl': '<S323>:452' */

4284
  if (((uint8_T *)ssGetDWork(S, 551))[0] == AFCS_MODEL1_IN_EmYrwControl) {

4285
    /* During 'EmYrwControl': '<S323>:493' */

4286
    if (((int8_T *)ssGetDWork(S, 513))[0] == 1) {

4287
      /* Transition: '<S323>:494' */

4288
      /* Exit Internal 'EmYrwControl': '<S323>:493' */

4289
      ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

4290


4291
      /* Exit Internal 'LevelPosition': '<S323>:567' */

4292
      ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

4293
      ((uint8_T *)ssGetDWork(S, 551))[0] = AFCS_MODEL1_IN_NormYrwControl;

4294


4295
      /* Entry 'NormYrwControl': '<S323>:454' */

4296
      /* Entry Internal 'NormYrwControl': '<S323>:454' */

4297
      /* Transition: '<S323>:455' */

4298
      ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_YrwControl1;

4299


4300
      /* Entry Internal 'YrwControl1': '<S323>:550' */

4301
      /* Transition: '<S323>:551' */

4302
      ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_phiLcontrol;

4303


4304
      /* Entry 'phiLcontrol': '<S323>:554' */

4305
      _rtB->aplatL_a = 5;

4306
      _rtB->apyawL_fh = 1;

4307
      _rtB->psdcmd_g = 0.0F;

4308
      ((int32_T *)ssGetDWork(S, 314))[0] = 0;

4309
    } else {

4310
      switch (((uint8_T *)ssGetDWork(S, 555))[0]) {

4311
       case AFCS_MODEL_IN_GPSFailPhiControl:

4312
        /* During 'GPSFailPhiControl': '<S323>:583' */

4313
        if (_rtB->GPSFail == 0) {

4314
          /* Transition: '<S323>:589' */

4315
          ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_LevelPosition;

4316


4317
          /* Entry Internal 'LevelPosition': '<S323>:567' */

4318
          /* Transition: '<S323>:568' */

4319
          ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_phiLcontrol2;

4320


4321
          /* Entry 'phiLcontrol2': '<S323>:571' */

4322
          _rtB->aplatL_a = 2;

4323
          _rtB->apyawL_fh = 1;

4324
          ((int32_T *)ssGetDWork(S, 315))[0] = 0;

4325
          ((int8_T *)ssGetDWork(S, 513))[0] = 0;

4326
        }

4327
        break;

4328


4329
       case AFCS_MODEL1_IN_LevelPosition:

4330
        /* During 'LevelPosition': '<S323>:567' */

4331
        if (_rtB->GPSFail == 1) {

4332
          /* Transition: '<S323>:588' */

4333
          /* Exit Internal 'LevelPosition': '<S323>:567' */

4334
          ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

4335
          ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL_IN_GPSFailPhiControl;

4336


4337
          /* Entry 'GPSFailPhiControl': '<S323>:583' */

4338
          _rtB->aplatL_a = 2;

4339
          _rtB->apyawL_fh = 1;

4340
          _rtB->phiLcmd_l4 = 30.0F;

4341
        } else if (((int32_T *)ssGetDWork(S, 316))[0] == 5) {

4342
          /* Transition: '<S323>:511' */

4343
          /* Exit Internal 'LevelPosition': '<S323>:567' */

4344
          ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

4345
          ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_PhiLControl3;

4346


4347
          /* Entry 'PhiLControl3': '<S323>:527' */

4348
          _rtB->aplatL_a = 2;

4349
          _rtB->apyawL_fh = 1;

4350
          ((int32_T *)ssGetDWork(S, 318))[0] = 0;

4351
        } else if (((uint8_T *)ssGetDWork(S, 556))[0] ==

4352
                   AFCS_MODEL1_IN_YrwControl_o) {

4353
          /* During 'YrwControl': '<S323>:577' */

4354
          ((int32_T *)ssGetDWork(S, 316))[0] = AFCS_MODEL1_addup_f((_rtB->drange

4355
            < 100.0F) && (_rtB->GPSFail == 0), 5, ((int32_T *)ssGetDWork(S, 316))

4356
            [0]);

4357


4358
          /* Transition: '<S323>:579' */

4359
          if (_rtB->yrw - _rtB->yrwcmd_i > 50.0F) {

4360
            /* Transition: '<S323>:581' */

4361
            _rtB->aplatL_a = 4;

4362
            _rtB->psdcmd_g = -10.0F;

4363
            _rtB->phiLcmd_l4 = 0.0F;

4364
          } else if (_rtB->yrw - _rtB->yrwcmd_i < -50.0F) {

4365
            /* Transition: '<S323>:582' */

4366
            _rtB->aplatL_a = 4;

4367
            _rtB->psdcmd_g = 10.0F;

4368
            _rtB->phiLcmd_l4 = 0.0F;

4369
          } else {

4370
            /* Transition: '<S323>:580' */

4371
            _rtB->aplatL_a = 5;

4372
            _rtB->yrwcmd_i = 0.0F;

4373
            _rtB->psdcmd_g = 0.0F;

4374
            _rtB->phiLcmd_l4 = 0.0F;

4375
          }

4376
        } else {

4377
          /* During 'phiLcontrol2': '<S323>:571' */

4378
          if ((((int32_T *)ssGetDWork(S, 315))[0] == 5) && (_rtB->SWAUTO == 1))

4379
          {

4380
            /* Transition: '<S323>:570' */

4381
            _rtB->StartEmNav_o = 1;

4382
            ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_YrwControl_o;

4383


4384
            /* Entry 'YrwControl': '<S323>:577' */

4385
            ((int32_T *)ssGetDWork(S, 316))[0] = 0;

4386
          } else {

4387
            ((int32_T *)ssGetDWork(S, 315))[0] = AFCS_MODEL1_addup_f((real32_T)

4388
              fabs(AFCS_MODEL1_res180_l(_rtB->DetaPsi)) < 15.0F, 5, ((int32_T *)

4389
              ssGetDWork(S, 315))[0]);

4390


4391
            /* Transition: '<S323>:573' */

4392
            if (AFCS_MODEL1_res180_l(_rtB->DetaPsi) > 15.0F) {

4393
              /* Transition: '<S323>:574' */

4394
              _rtB->aplatL_a = 2;

4395
              _rtB->phiLcmd_l4 = 30.0F;

4396
            } else {

4397
              if (AFCS_MODEL1_res180_l(_rtB->DetaPsi) < -15.0F) {

4398
                /* Transition: '<S323>:576' */

4399
                _rtB->aplatL_a = 2;

4400
                _rtB->phiLcmd_l4 = -30.0F;

4401
              } else {

4402
                /* Transition: '<S323>:575' */

4403
                _rtB->aplatL_a = 2;

4404
                _rtB->phiLcmd_l4 = 0.0F;

4405
              }

4406
            }

4407
          }

4408
        }

4409
        break;

4410


4411
       case AFCS_MODEL1_IN_PhiLControl3:

4412
        /* During 'PhiLControl3': '<S323>:527' */

4413
        if (((int32_T *)ssGetDWork(S, 318))[0] == 5) {

4414
          /* Transition: '<S323>:536' */

4415
          ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_TranseNorm;

4416


4417
          /* Entry 'TranseNorm': '<S323>:510' */

4418
          _rtB->StartEmNav_o = 0;

4419
          ((int8_T *)ssGetDWork(S, 513))[0] = 1;

4420
          tmp = ((int32_T *)ssGetDWork(S, 317))[0] + 1LL;

4421
          if (tmp > 2147483647LL) {

4422
            tmp = 2147483647LL;

4423
          } else {

4424
            if (tmp < -2147483648LL) {

4425
              tmp = -2147483648LL;

4426
            }

4427
          }

4428


4429
          ((int32_T *)ssGetDWork(S, 317))[0] = (int32_T)tmp;

4430
        } else {

4431
          _rtB->phiLcmd_l4 = 30.0F * _rtB->DireFg;

4432
          ((int32_T *)ssGetDWork(S, 318))[0] = AFCS_MODEL1_addup_f((real32_T)

4433
            fabs(AFCS_MODEL1_res180_l(_rtB->DetaPsi)) < 15.0F, 5, ((int32_T *)

4434
            ssGetDWork(S, 318))[0]);

4435
        }

4436
        break;

4437


4438
       default:

4439
        /* During 'TranseNorm': '<S323>:510' */

4440
        break;

4441
      }

4442
    }

4443
  } else {

4444
    /* During 'NormYrwControl': '<S323>:454' */

4445
    if ((_rtB->EmlandFg == 1) && (((int32_T *)ssGetDWork(S, 317))[0] == 0) &&

4446
        (_rtB->GPSFail == 0)) {

4447
      /* Transition: '<S323>:495' */

4448
      /* Exit Internal 'NormYrwControl': '<S323>:454' */

4449
      ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

4450


4451
      /* Exit Internal 'YrwControl1': '<S323>:550' */

4452
      ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

4453
      ((uint8_T *)ssGetDWork(S, 551))[0] = AFCS_MODEL1_IN_EmYrwControl;

4454


4455
      /* Entry Internal 'EmYrwControl': '<S323>:493' */

4456
      /* Transition: '<S323>:502' */

4457
      ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_LevelPosition;

4458


4459
      /* Entry Internal 'LevelPosition': '<S323>:567' */

4460
      /* Transition: '<S323>:568' */

4461
      ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_phiLcontrol2;

4462


4463
      /* Entry 'phiLcontrol2': '<S323>:571' */

4464
      _rtB->aplatL_a = 2;

4465
      _rtB->apyawL_fh = 1;

4466
      ((int32_T *)ssGetDWork(S, 315))[0] = 0;

4467
      ((int8_T *)ssGetDWork(S, 513))[0] = 0;

4468
    } else if (((uint8_T *)ssGetDWork(S, 553))[0] ==

4469
               AFCS_MODEL_IN_GPSFailPhiControl) {

4470
      /* During 'GPSFailPhiControl': '<S323>:545' */

4471
      if (_rtB->GPSFail == 0) {

4472
        /* Transition: '<S323>:544' */

4473
        ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_YrwControl1;

4474


4475
        /* Entry Internal 'YrwControl1': '<S323>:550' */

4476
        /* Transition: '<S323>:551' */

4477
        ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_phiLcontrol;

4478


4479
        /* Entry 'phiLcontrol': '<S323>:554' */

4480
        _rtB->aplatL_a = 5;

4481
        _rtB->apyawL_fh = 1;

4482
        _rtB->psdcmd_g = 0.0F;

4483
        ((int32_T *)ssGetDWork(S, 314))[0] = 0;

4484
      }

4485
    } else {

4486
      /* During 'YrwControl1': '<S323>:550' */

4487
      if (_rtB->GPSFail == 1) {

4488
        /* Transition: '<S323>:543' */

4489
        /* Exit Internal 'YrwControl1': '<S323>:550' */

4490
        ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

4491
        ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL_IN_GPSFailPhiControl;

4492


4493
        /* Entry 'GPSFailPhiControl': '<S323>:545' */

4494
        _rtB->aplatL_a = 2;

4495
        _rtB->apyawL_fh = 1;

4496
        _rtB->phiLcmd_l4 = 30.0F;

4497
      } else if (((uint8_T *)ssGetDWork(S, 554))[0] ==

4498
                 AFCS_MODEL1_IN_YrwControl_o) {

4499
        /* During 'YrwControl': '<S323>:560' */

4500
        /* Transition: '<S323>:562' */

4501
        if (_rtB->yrw - _rtB->yrwcmd_i > 50.0F) {

4502
          /* Transition: '<S323>:564' */

4503
          _rtB->aplatL_a = 4;

4504
          _rtB->psdcmd_g = -10.0F;

4505
          _rtB->phiLcmd_l4 = 0.0F;

4506
        } else if (_rtB->yrw - _rtB->yrwcmd_i < -50.0F) {

4507
          /* Transition: '<S323>:565' */

4508
          _rtB->aplatL_a = 4;

4509
          _rtB->psdcmd_g = 10.0F;

4510
          _rtB->phiLcmd_l4 = 0.0F;

4511
        } else {

4512
          /* Transition: '<S323>:563' */

4513
          _rtB->aplatL_a = 5;

4514
          _rtB->yrwcmd_i = 0.0F;

4515
          _rtB->psdcmd_g = 0.0F;

4516
          _rtB->phiLcmd_l4 = 0.0F;

4517
        }

4518
      } else {

4519
        /* During 'phiLcontrol': '<S323>:554' */

4520
        if ((((int32_T *)ssGetDWork(S, 314))[0] == 5) && (_rtB->SWAUTO == 1)) {

4521
          /* Transition: '<S323>:552' */

4522
          ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_YrwControl_o;

4523
        } else {

4524
          ((int32_T *)ssGetDWork(S, 314))[0] = AFCS_MODEL1_addup_f((real32_T)

4525
            fabs(AFCS_MODEL1_res180_l(_rtB->psdcmd_g - _rtB->psd)) < 15.0F, 5,

4526
            ((int32_T *)ssGetDWork(S, 314))[0]);

4527


4528
          /* Transition: '<S323>:556' */

4529
          if (AFCS_MODEL1_res180_l(_rtB->psdcmd_g - _rtB->psd) > 20.0F) {

4530
            /* Transition: '<S323>:557' */

4531
            _rtB->aplatL_a = 2;

4532
            _rtB->phiLcmd_l4 = 30.0F;

4533
          } else {

4534
            if (AFCS_MODEL1_res180_l(_rtB->psdcmd_g - _rtB->psd) < -20.0F) {

4535
              /* Transition: '<S323>:559' */

4536
              _rtB->aplatL_a = 2;

4537
              _rtB->phiLcmd_l4 = -30.0F;

4538
            } else {

4539
              /* Transition: '<S323>:558' */

4540
              _rtB->aplatL_a = 4;

4541
              _rtB->psdcmd_g = 0.0F;

4542
              _rtB->phiLcmd_l4 = 0.0F;

4543
            }

4544
          }

4545
        }

4546
      }

4547
    }

4548
  }

4549
}

4550


4551
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

4552
static void AFCS_MODEL1_tand(real_T *x)

4553
{

4554
  int8_T n;

4555
  static real_T b_x;

4556
  static real_T absx;

4557
  if (!!rtIsInf(*x)) {

4558
    *x = (rtNaN);

4559
  } else {

4560
    b_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);

4561
    absx = fabs(b_x);

4562
    if (absx > 180.0) {

4563
      if (b_x > 0.0) {

4564
        b_x -= 360.0;

4565
      } else {

4566
        b_x += 360.0;

4567
      }

4568


4569
      absx = fabs(b_x);

4570
    }

4571


4572
    if (absx <= 45.0) {

4573
      b_x *= 0.017453292519943295;

4574
      n = 0;

4575
    } else if (absx <= 135.0) {

4576
      if (b_x > 0.0) {

4577
        b_x = (b_x - 90.0) * 0.017453292519943295;

4578
        n = 1;

4579
      } else {

4580
        b_x = (b_x + 90.0) * 0.017453292519943295;

4581
        n = -1;

4582
      }

4583
    } else if (b_x > 0.0) {

4584
      b_x = (b_x - 180.0) * 0.017453292519943295;

4585
      n = 2;

4586
    } else {

4587
      b_x = (b_x + 180.0) * 0.017453292519943295;

4588
      n = -2;

4589
    }

4590


4591
    *x = tan(b_x);

4592
    if ((n == 1) || (n == -1)) {

4593
      b_x = 1.0 / *x;

4594
      *x = -(1.0 / *x);

4595
      if (rtIsInf(*x) && (n == 1)) {

4596
        *x = -(-b_x);

4597
      }

4598
    }

4599
  }

4600
}

4601


4602
/* Function for Chart: '<S314>/Climb2' */

4603
static real32_T AFCS_MODEL1_res180(real32_T angle)

4604
{

4605
  real32_T outangle;

4606


4607
  /* MATLAB Function 'res180': '<S324>:229' */

4608
  /* Graphical Function 'res180': '<S324>:229' */

4609
  /* '<S324>:235:1' */

4610
  if (angle > 180.0F) {

4611
    /* '<S324>:235:1' */

4612
    outangle = angle - 360.0F;

4613
  } else {

4614
    /* '<S324>:237:1' */

4615
    if (angle < -180.0F) {

4616
      /* '<S324>:237:1' */

4617
      outangle = angle + 360.0F;

4618
    } else {

4619
      /* '<S324>:236:1' */

4620
      outangle = angle;

4621
    }

4622
  }

4623


4624
  return outangle;

4625
}

4626


4627
/* Function for Chart: '<S315>/Cruise' */

4628
static int32_T AFCS_MODEL1_addup_o(boolean_T bA, int32_T iB, int32_T iC)

4629
{

4630
  int32_T tC;

4631
  int64_T tmp;

4632


4633
  /* MATLAB Function 'addup': '<S325>:711' */

4634
  /* Graphical Function 'addup': '<S325>:711' */

4635
  /* '<S325>:719:1' */

4636
  tC = iC;

4637


4638
  /* '<S325>:720:1' */

4639
  if (!bA) {

4640
    /* '<S325>:723:1' */

4641
    if (iC > 0) {

4642
      /* '<S325>:723:1' */

4643
      tC = iC - 1;

4644
    }

4645
  } else {

4646
    /* '<S325>:725:1' */

4647
    if (iC > iB) {

4648
      /* '<S325>:725:1' */

4649
      tC = iB;

4650
    } else {

4651
      /* '<S325>:724:1' */

4652
      tmp = iC + 1LL;

4653
      if (tmp > 2147483647LL) {

4654
        tmp = 2147483647LL;

4655
      } else {

4656
        if (tmp < -2147483648LL) {

4657
          tmp = -2147483648LL;

4658
        }

4659
      }

4660


4661
      tC = (int32_T)tmp;

4662
    }

4663
  }

4664


4665
  return tC;

4666
}

4667


4668
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

4669
static void AFCS_MODEL1_PGT_CalculateRmin(const real_T xValues[32], const real_T

4670
  yValues[32], int8_T IndataLength, real_T Rmin[32], real_T b_RminNew[255])

4671
{

4672
  static real_T dcwp[255];

4673
  static real_T dwp;

4674
  static real_T ax;

4675
  static real_T ay;

4676
  static real_T bx;

4677
  static real_T by;

4678
  static real_T Cthet;

4679
  int32_T count;

4680
  int8_T i;

4681
  int32_T tmp;

4682


4683
  /* UNTITLED3 此处显示有关此函数的摘要 */

4684
  /*    此处显示详细说明 */

4685
  /* 对转弯半径进行微调，避免转弯弧线有重叠 */

4686
  memset(&dcwp[0], 0, 255U * sizeof(real_T));

4687
  tmp = IndataLength - 2;

4688
  if (tmp < -128) {

4689
    tmp = -128;

4690
  }

4691


4692
  for (i = 1; i <= (int8_T)tmp; i++) {

4693
    dwp = sqrt((xValues[i] - xValues[i - 1]) * (xValues[i] - xValues[i - 1]) +

4694
               (yValues[i] - yValues[i - 1]) * (yValues[i] - yValues[i - 1]));

4695


4696
    /*      (ax,ay)为中间航点指向前一航点的向量 */

4697
    ax = xValues[i - 1] - xValues[i];

4698
    ay = yValues[i - 1] - yValues[i];

4699


4700
    /*  		(bx,by)为中间航点指向后一航点的向量 */

4701
    bx = xValues[i + 1] - xValues[i];

4702
    by = yValues[i + 1] - yValues[i];

4703
    if (sqrt(ax * ax + ay * ay) * sqrt(bx * bx + by * by) < 1.0E-7) {

4704
      /* 若有两点重合 */

4705
      Cthet = 1.0E+7;

4706
    } else {

4707
      Cthet = (ax * bx + ay * by) / (sqrt(ax * ax + ay * ay) * sqrt(bx * bx + by

4708
        * by));

4709


4710
      /* //两向量夹角的余弦值 */

4711
      if (1.0 + Cthet < 1.0E-7) {

4712
        Cthet = 1.0E+7;

4713
      } else {

4714
        Cthet = sqrt((1.0 - Cthet) / (1.0 + Cthet));

4715


4716
        /* //两向量夹角一半的正切值 */

4717
      }

4718
    }

4719


4720
    /*  //当三个航点有两个重合，或者两向量夹角为pi，或者两向量夹角为零 */

4721
    if ((sqrt(ax * ax + ay * ay) * sqrt(bx * bx + by * by) < 1.0E-7) || (fabs

4722
         (atan(Cthet) - 1.5707963267948966) < 1.0E-7) || (fabs(Cthet) < 0.1)) {

4723
      /* 2018/10/22修改 */

4724
      dcwp[i] = 0.0;

4725


4726
      /* //在这三个航点处转弯所需的路径长度为零 */

4727
    } else {

4728
      dcwp[i] = Rmin[i] / Cthet;

4729


4730
      /* //在这三个航点处转弯所需的路径长度为最小转弯半径除以夹角一半的正切值 */

4731
    }

4732


4733
    /* 判断当前转弯半径是否满足条件，若不满足，则减小转弯半径 */

4734
    count = 0;

4735
    while ((dwp < dcwp[i - 1] + dcwp[i]) && (count < 100)) {

4736
      /* 2018/10/09修改 */

4737
      Rmin[i] *= 0.95;

4738
      Rmin[i - 1] *= 0.95;

4739
      dcwp[i - 1] *= 0.95;

4740
      dcwp[i] *= 0.95;

4741
      count++;

4742
    }

4743
  }

4744


4745
  memset(&b_RminNew[0], 0, 255U * sizeof(real_T));

4746
  for (i = 1; i <= IndataLength; i++) {

4747
    b_RminNew[i - 1] = Rmin[i - 1];

4748
  }

4749
}

4750


4751
static void rt_mrdivide_U1d1x3_U2d_9vOrDY_k(const real_T u0[3], const real_T u1

4752
  [9], real_T y[3])

4753
{

4754
  static real_T A[9];

4755
  int32_T r1;

4756
  int32_T r2;

4757
  int32_T r3;

4758
  static real_T maxval;

4759
  static real_T a21;

4760
  int32_T rtemp;

4761
  memcpy(&A[0], &u1[0], 9U * sizeof(real_T));

4762
  r1 = 0;

4763
  r2 = 1;

4764
  r3 = 2;

4765
  maxval = fabs(u1[0]);

4766
  a21 = fabs(u1[1]);

4767
  if (a21 > maxval) {

4768
    maxval = a21;

4769
    r1 = 1;

4770
    r2 = 0;

4771
  }

4772


4773
  if (fabs(u1[2]) > maxval) {

4774
    r1 = 2;

4775
    r2 = 1;

4776
    r3 = 0;

4777
  }

4778


4779
  A[r2] = u1[r2] / u1[r1];

4780
  A[r3] /= A[r1];

4781
  A[3 + r2] -= A[3 + r1] * A[r2];

4782
  A[3 + r3] -= A[3 + r1] * A[r3];

4783
  A[6 + r2] -= A[6 + r1] * A[r2];

4784
  A[6 + r3] -= A[6 + r1] * A[r3];

4785
  if (fabs(A[3 + r3]) > fabs(A[3 + r2])) {

4786
    rtemp = r2 + 1;

4787
    r2 = r3;

4788
    r3 = rtemp - 1;

4789
  }

4790


4791
  A[3 + r3] /= A[3 + r2];

4792
  A[6 + r3] -= A[3 + r3] * A[6 + r2];

4793
  y[r1] = u0[0] / A[r1];

4794
  y[r2] = u0[1] - A[3 + r1] * y[r1];

4795
  y[r3] = u0[2] - A[6 + r1] * y[r1];

4796
  y[r2] /= A[3 + r2];

4797
  y[r3] -= A[6 + r2] * y[r2];

4798
  y[r3] /= A[6 + r3];

4799
  y[r2] -= A[3 + r3] * y[r3];

4800
  y[r1] -= y[r3] * A[r3];

4801
  y[r1] -= y[r2] * A[r2];

4802
}

4803


4804
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

4805
static void AFCS_MODEL1_PGT_changewaypoints(const real_T xValues[32], const

4806
  real_T yValues[32], const real_T hValues[32], int8_T IndataLength, const

4807
  real_T Rmin[255], const real_T vValues[32], real_T vx_qiedian_out[255], real_T

4808
  vy_qiedian_out[255], real_T xn_qiedian_out[255], real_T ye_qiedian_out[255],

4809
  real_T zh_qiedian_out[255])

4810
{

4811
  static real_T K;

4812
  static real_T d_to_front;

4813
  static real_T q1x;

4814
  static real_T q1y;

4815
  static real_T d_to_next;

4816
  static real_T q2x;

4817
  static real_T q2y;

4818
  static real_T Anglerou;

4819
  static real_T Angleroucos;

4820
  int8_T i;

4821
  static real_T a;

4822
  int32_T tmp;

4823
  int32_T tmp_0;

4824


4825
  /* 输入： */

4826
  /* 1.xValues：航点的x值 */

4827
  /* 2.yValues：航点的y值 */

4828
  /* 3.zValues：航点的z值 */

4829
  /* 4.IndataLength：航点的个数 */

4830
  /* 5.Rmin：各个航点的最小转弯半径 */

4831
  /* 6.vValues：每个航点的速度大小 */

4832
  /* 输出： */

4833
  /* 1.vx:切点的x方向速度值 */

4834
  /* 2.vy:切点的y方向速度值 */

4835
  /* 3.xx:切点的x方向位置 */

4836
  /* 4.yy:切点的y方向位置 */

4837
  /* 5.zz:切点的z方向位置 */

4838
  /* 备注：由三个航点（i-1,i,i+1)可以确定i航点的切点，这里的切点指i与i+1航点之间的切点 */

4839
  vx_qiedian_out[254] = 0.0;

4840
  vx_qiedian_out[0] = 0.0;

4841
  vy_qiedian_out[254] = 0.0;

4842
  vy_qiedian_out[0] = 0.0;

4843
  xn_qiedian_out[254] = 0.0;

4844
  xn_qiedian_out[0] = 0.0;

4845
  ye_qiedian_out[254] = 0.0;

4846
  ye_qiedian_out[0] = 0.0;

4847
  memset(&zh_qiedian_out[0], 0, 255U * sizeof(real_T));

4848
  memset(&vx_qiedian_out[1], 0, 253U * sizeof(real_T));

4849
  memset(&vy_qiedian_out[1], 0, 253U * sizeof(real_T));

4850
  memset(&xn_qiedian_out[1], 0, 253U * sizeof(real_T));

4851
  memset(&ye_qiedian_out[1], 0, 253U * sizeof(real_T));

4852


4853
  /* % */

4854
  for (i = 1; i <= IndataLength; i++) {

4855
    /*  velocity */

4856
    /* 还是对于垂直于x轴的情况要单独处理 */

4857
    tmp_0 = i + 1;

4858
    if (tmp_0 > 127) {

4859
      tmp_0 = 127;

4860
    }

4861


4862
    if (fabs(xValues[tmp_0 - 1] - xValues[i - 1]) < 1.0E-7) {

4863
      vx_qiedian_out[i - 1] = 0.0;

4864
      tmp_0 = i + 1;

4865
      if (tmp_0 > 127) {

4866
        tmp_0 = 127;

4867
      }

4868


4869
      if (fabs(yValues[tmp_0 - 1] - yValues[i - 1]) < 1.0E-7) {

4870
        vy_qiedian_out[i - 1] = 0.0;

4871
      } else {

4872
        tmp_0 = i + 1;

4873
        if (tmp_0 > 127) {

4874
          tmp_0 = 127;

4875
        }

4876


4877
        if (yValues[tmp_0 - 1] - yValues[i - 1] > 0.0) {

4878
          vy_qiedian_out[i - 1] = vValues[i - 1];

4879
        } else {

4880
          vy_qiedian_out[i - 1] = -vValues[i - 1];

4881
        }

4882
      }

4883
    } else {

4884
      tmp_0 = i + 1;

4885
      if (tmp_0 > 127) {

4886
        tmp_0 = 127;

4887
      }

4888


4889
      tmp = i + 1;

4890
      if (tmp > 127) {

4891
        tmp = 127;

4892
      }

4893


4894
      K = (yValues[tmp_0 - 1] - yValues[i - 1]) / (xValues[tmp - 1] - xValues[i

4895
        - 1]);

4896
      tmp_0 = i + 1;

4897
      if (tmp_0 > 127) {

4898
        tmp_0 = 127;

4899
      }

4900


4901
      if (xValues[tmp_0 - 1] - xValues[i - 1] >= 0.0) {

4902
        vx_qiedian_out[i - 1] = vValues[i - 1] / sqrt(K * K + 1.0);

4903
      } else {

4904
        vx_qiedian_out[i - 1] = -vValues[i - 1] / sqrt(K * K + 1.0);

4905
      }

4906


4907
      tmp_0 = i + 1;

4908
      if (tmp_0 > 127) {

4909
        tmp_0 = 127;

4910
      }

4911


4912
      if (yValues[tmp_0 - 1] - yValues[i - 1] >= 0.0) {

4913
        vy_qiedian_out[i - 1] = vValues[i - 1] / sqrt(K * K + 1.0) * fabs(K);

4914
      } else {

4915
        vy_qiedian_out[i - 1] = -vValues[i - 1] / sqrt(K * K + 1.0) * fabs(K);

4916
      }

4917
    }

4918


4919
    /* ===================================================== */

4920
    /*                     a*b             x1*x2+y1*y2 */

4921
    /*  COS(theta) =----------------- =  --------------- */

4922
    /*                   |a|*|b|            |a|*|b| */

4923
    /* ===================================================== */

4924
    /*  //waypoints// */

4925
    if ((i == 1) || (i == IndataLength)) {

4926
      xn_qiedian_out[i - 1] = xValues[i - 1];

4927


4928
      /* yht:第一个航点和最后一个航点的切点位置就是航点本身 */

4929
      ye_qiedian_out[i - 1] = yValues[i - 1];

4930
      zh_qiedian_out[i - 1] = hValues[i - 1];

4931
    } else {

4932
      tmp_0 = i - 1;

4933
      if (tmp_0 < -128) {

4934
        tmp_0 = -128;

4935
      }

4936


4937
      a = xValues[i - 1] - xValues[tmp_0 - 1];

4938
      tmp_0 = i - 1;

4939
      if (tmp_0 < -128) {

4940
        tmp_0 = -128;

4941
      }

4942


4943
      d_to_front = yValues[i - 1] - yValues[tmp_0 - 1];

4944
      d_to_front = sqrt(a * a + d_to_front * d_to_front);

4945
      if (d_to_front < 1.0E-7) {

4946
        /* 当前航点与上一航点重合 */

4947
        q1x = 0.0;

4948
        q1y = 0.0;

4949
      } else {

4950
        tmp_0 = i - 1;

4951
        if (tmp_0 < -128) {

4952
          tmp_0 = -128;

4953
        }

4954


4955
        q1x = (xValues[tmp_0 - 1] - xValues[i - 1]) / d_to_front;

4956


4957
        /* COS倾斜角（连线与x轴的夹角）,归一化的相对坐标 */

4958
        tmp_0 = i - 1;

4959
        if (tmp_0 < -128) {

4960
          tmp_0 = -128;

4961
        }

4962


4963
        q1y = (yValues[tmp_0 - 1] - yValues[i - 1]) / d_to_front;

4964


4965
        /* SIN倾斜角 */

4966
      }

4967


4968
      tmp_0 = i + 1;

4969
      if (tmp_0 > 127) {

4970
        tmp_0 = 127;

4971
      }

4972


4973
      d_to_next = xValues[tmp_0 - 1] - xValues[i - 1];

4974
      tmp_0 = i + 1;

4975
      if (tmp_0 > 127) {

4976
        tmp_0 = 127;

4977
      }

4978


4979
      q2x = yValues[tmp_0 - 1] - yValues[i - 1];

4980
      d_to_next = sqrt(d_to_next * d_to_next + q2x * q2x);

4981
      if (d_to_next < 1.0E-7) {

4982
        q2x = 0.0;

4983
        q2y = 0.0;

4984
      } else {

4985
        tmp_0 = i + 1;

4986
        if (tmp_0 > 127) {

4987
          tmp_0 = 127;

4988
        }

4989


4990
        q2x = (xValues[tmp_0 - 1] - xValues[i - 1]) / d_to_next;

4991
        tmp_0 = i + 1;

4992
        if (tmp_0 > 127) {

4993
          tmp_0 = 127;

4994
        }

4995


4996
        q2y = (yValues[tmp_0 - 1] - yValues[i - 1]) / d_to_next;

4997
      }

4998


4999
      if ((fabs(q1x) + fabs(q1y) < 1.0E-7) || (fabs(q2x) + fabs(q2y) < 1.0E-7))

5000
      {

5001
        /* 如果有航点是重合的，则夹角是0 */

5002
        Anglerou = 0.0;

5003
      } else {

5004
        Angleroucos = q1x * q2x + q1y * q2y;

5005
        if (Angleroucos > 1.0) {

5006
          Angleroucos = 1.0;

5007
        } else {

5008
          if (Angleroucos < -1.0) {

5009
            Angleroucos = -1.0;

5010
          }

5011
        }

5012


5013
        Anglerou = acos(Angleroucos);

5014
      }

5015


5016
      /*         %% */

5017
      /* 2018/10/09修改 */

5018
      if ((fabs(Anglerou) < 0.087266462599716474) || (3.1415926535897931 -

5019
           Anglerou < 0.087266462599716474)) {

5020
        /* 两直线夹角小于5度时切点为原始航点 */

5021
        xn_qiedian_out[i - 1] = xValues[i - 1];

5022
        ye_qiedian_out[i - 1] = yValues[i - 1];

5023
      } else {

5024
        xn_qiedian_out[i - 1] = Rmin[i - 1] / tan(Anglerou / 2.0) * q2x +

5025
          xValues[i - 1];

5026
        ye_qiedian_out[i - 1] = Rmin[i - 1] / tan(Anglerou / 2.0) * q2y +

5027
          yValues[i - 1];

5028
      }

5029


5030
      /*          dd = sqrt((xx(i) - xValues(i))*(xx(i) - xValues(i)) + (yy(i) - yValues(i))*(yy(i) - yValues(i)));     %切点到航点的距离 */

5031
      /*          L = sqrt((xValues(i + 1) - xValues(i))*(xValues(i + 1) - xValues(i)) + (yValues(i + 1) - yValues(i))*(yValues(i + 1) - yValues(i)));   %两个航点的距离 */

5032
      zh_qiedian_out[i - 1] = hValues[i - 1];

5033


5034
      /*  + dd / L*(zValues(i + 1) - zValues(i));    %切点的高度,yanhongtao,20210220 */

5035
    }

5036
  }

5037
}

5038


5039
/* Function for Chart: '<S21>/delayblock1' */

5040
static int32_T AFCS_MODEL1_addup(int32_T bA, int32_T iB, int32_T iC)

5041
{

5042
  int32_T tC;

5043
  int64_T tmp;

5044


5045
  /* MATLAB Function 'addup': '<S48>:114' */

5046
  /* Graphical Function 'addup': '<S48>:114' */

5047
  /* '<S48>:122:1' */

5048
  tC = iC;

5049


5050
  /* '<S48>:123:1' */

5051
  if (bA == 0) {

5052
    /* '<S48>:126:1' */

5053
    if (iC > 0) {

5054
      /* '<S48>:126:1' */

5055
      tC = iC - 1;

5056
    }

5057
  } else {

5058
    /* '<S48>:128:1' */

5059
    if (iC > iB) {

5060
      /* '<S48>:128:1' */

5061
      tC = iB;

5062
    } else {

5063
      /* '<S48>:127:1' */

5064
      tmp = iC + 1LL;

5065
      if (tmp > 2147483647LL) {

5066
        tmp = 2147483647LL;

5067
      } else {

5068
        if (tmp < -2147483648LL) {

5069
          tmp = -2147483648LL;

5070
        }

5071
      }

5072


5073
      tC = (int32_T)tmp;

5074
    }

5075
  }

5076


5077
  return tC;

5078
}

5079


5080
/* Function for MATLAB Function: '<S293>/CompTP' */

5081
static void AFCS_MODEL1_CompDistance(real32_T X1, real32_T Y1, real32_T X2,

5082
  real32_T Y2, real32_T Xac, real32_T Yac, real32_T *DD, real32_T *A, real32_T

5083
  *Xrw, real32_T *Yrw)

5084
{

5085
  real32_T DX;

5086
  real32_T DY;

5087
  real32_T DXp;

5088
  real32_T DYp;

5089


5090
  /* '<S295>:1:138' */

5091
  DX = X2 - X1;

5092


5093
  /* '<S295>:1:139' */

5094
  DY = Y2 - Y1;

5095


5096
  /* '<S295>:1:140' */

5097
  DXp = Xac - X1;

5098


5099
  /* '<S295>:1:141' */

5100
  DYp = Yac - Y1;

5101


5102
  /* '<S295>:1:142' */

5103
  *DD = (real32_T)sqrt(DX * DX + DY * DY);

5104


5105
  /* '<S295>:1:143' */

5106
  *A = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;

5107


5108
  /* '<S295>:1:144' */

5109
  DX /= *DD;

5110


5111
  /* '<S295>:1:145' */

5112
  DY /= *DD;

5113


5114
  /* '<S295>:1:146' */

5115
  *Yrw = DYp * DX - DXp * DY;

5116


5117
  /* '<S295>:1:147' */

5118
  *Xrw = *DD - (DXp * DX + DYp * DY);

5119
}

5120


5121
/* Function for Chart: '<S21>/delayblock6' */

5122
static int32_T AFCS_MODEL1_addup_m(int32_T bA, int32_T iB, int32_T iC)

5123
{

5124
  int32_T tC;

5125
  int64_T tmp;

5126


5127
  /* MATLAB Function 'addup': '<S53>:114' */

5128
  /* Graphical Function 'addup': '<S53>:114' */

5129
  /* '<S53>:122:1' */

5130
  tC = iC;

5131


5132
  /* '<S53>:123:1' */

5133
  if (bA == 0) {

5134
    /* '<S53>:126:1' */

5135
    if (iC > 0) {

5136
      /* '<S53>:126:1' */

5137
      tC = iC - 1;

5138
    }

5139
  } else {

5140
    /* '<S53>:128:1' */

5141
    if (iC > iB) {

5142
      /* '<S53>:128:1' */

5143
      tC = iB;

5144
    } else {

5145
      /* '<S53>:127:1' */

5146
      tmp = iC + 1LL;

5147
      if (tmp > 2147483647LL) {

5148
        tmp = 2147483647LL;

5149
      } else {

5150
        if (tmp < -2147483648LL) {

5151
          tmp = -2147483648LL;

5152
        }

5153
      }

5154


5155
      tC = (int32_T)tmp;

5156
    }

5157
  }

5158


5159
  return tC;

5160
}

5161


5162
/* Function for MATLAB Function: '<S293>/CompTP' */

5163
static void AFCS_MODEL1_cosd(real32_T *x)

5164
{

5165
  int8_T n;

5166
  real32_T b_x;

5167
  real32_T absx;

5168
  if (!((!rtIsInfF(*x)) && (!rtIsNaNF(*x)))) {

5169
    *x = (rtNaNF);

5170
  } else {

5171
    b_x = AFCS_MODEL1_rt_remf_snf(*x, 360.0F);

5172
    absx = (real32_T)fabs(b_x);

5173
    if (absx > 180.0F) {

5174
      if (b_x > 0.0F) {

5175
        b_x -= 360.0F;

5176
      } else {

5177
        b_x += 360.0F;

5178
      }

5179


5180
      absx = (real32_T)fabs(b_x);

5181
    }

5182


5183
    if (absx <= 45.0F) {

5184
      b_x *= 0.0174532924F;

5185
      n = 0;

5186
    } else if (absx <= 135.0F) {

5187
      if (b_x > 0.0F) {

5188
        b_x = (b_x - 90.0F) * 0.0174532924F;

5189
        n = 1;

5190
      } else {

5191
        b_x = (b_x + 90.0F) * 0.0174532924F;

5192
        n = -1;

5193
      }

5194
    } else if (b_x > 0.0F) {

5195
      b_x = (b_x - 180.0F) * 0.0174532924F;

5196
      n = 2;

5197
    } else {

5198
      b_x = (b_x + 180.0F) * 0.0174532924F;

5199
      n = -2;

5200
    }

5201


5202
    if (n == 0) {

5203
      *x = (real32_T)cos(b_x);

5204
    } else if (n == 1) {

5205
      *x = -(real32_T)sin(b_x);

5206
    } else if (n == -1) {

5207
      *x = (real32_T)sin(b_x);

5208
    } else {

5209
      *x = -(real32_T)cos(b_x);

5210
    }

5211
  }

5212
}

5213


5214
/* Function for MATLAB Function: '<S335>/UAV Trajectory Tracking' */

5215
static void PGT_followOrbit_nonlinear_guida(real_T cn, real_T ce, real_T roup,

5216
  real_T lamda, real_T pn, real_T pe, real_T vn, real_T ve, real_T psi, real_T

5217
  zz, real_T nav_param, real_T *hc, real_T *psic, real_T *acmd)

5218
{

5219
  static real_T Vg;

5220
  static real_T L1;

5221
  static real_T d;

5222
  static real_T dthet;

5223
  real_T fi;

5224


5225
  /* UNTITLED3 此处显示有关此函数的摘要 */

5226
  /* Input: Orbit center c = (cn, ce, cd)T , radius roup, and direction lamda, MAV position p = (pn, pe, pd)T , */

5227
  /* course psi, gains korbit. */

5228
  /* 输入： */

5229
  /* cn,ce:圆的圆心坐标x,y */

5230
  /* roup:圆的半径 */

5231
  /* lamda：圆的转动方向(顺时针还是逆时针） */

5232
  /* pn,pe，vn,ve：飞机的当前位置和速度 */

5233
  /* psi:飞机的速度偏角 */

5234
  /* zz:飞机当前转弯点的z */

5235
  /* nav_param：导航参数 */

5236
  /*    此处显示详细说明 */

5237
  Vg = sqrt(vn * vn + ve * ve);

5238


5239
  /* 飞机的速度 */

5240
  L1 = nav_param * Vg;

5241


5242
  /* 飞机到预瞄点的距离 */

5243
  d = pn - cn;

5244
  dthet = pe - ce;

5245
  d = sqrt(d * d + dthet * dthet);

5246


5247
  /* 飞机到圆心的距离 */

5248
  if (fabs(((d * d + roup * roup) - L1 * L1) / (2.0 * d * roup)) < 1.0) {

5249
    dthet = acos(((d * d + roup * roup) - L1 * L1) / (2.0 * d * roup));

5250
  } else {

5251
    dthet = 0.005;

5252
  }

5253


5254
  *hc = -zz;

5255


5256
  /* 指令高度为当前转弯点的高度 */

5257
  fi = AFCS_MODEL1_rt_atan2d_snf(pe - ce, pn - cn);

5258


5259
  /* 飞机与圆心的连线与北向的夹角 */

5260
  if (fi < 0.0) {

5261
    fi += 6.2831853071795862;

5262
  }

5263


5264
  d = cos(lamda * dthet + fi) * roup + cn;

5265


5266
  /* 预瞄点的坐标x */

5267
  dthet = sin(lamda * dthet + fi) * roup + ce;

5268


5269
  /* 预瞄点的坐标y */

5270
  /* 飞机实际到预瞄点的距离 */

5271
  *psic = AFCS_MODEL1_rt_atan2d_snf(dthet - pe, d - pn);

5272


5273
  /* atan2的范围是-pi~pi,飞机到预瞄点连线与北向的夹角 */

5274
  if (*psic < 0.0) {

5275
    *psic += 6.2831853071795862;

5276
  }

5277


5278
  /* 速度与飞机预瞄点连线的夹角 */

5279
  *acmd = 2.0 * Vg * Vg / sqrt((pn - d) * (pn - d) + (pe - dthet) * (pe - dthet))

5280
    * sin(*psic - psi);

5281


5282
  /* 非线性制导 */

5283
  if (*psic - psi < -3.1415926535897931) {

5284
    *psic += 6.2831853071795862;

5285
  }

5286


5287
  if (*psic - psi > 3.1415926535897931) {

5288
    *psic -= 6.2831853071795862;

5289
  }

5290
}

5291


5292
/* Function for MATLAB Function: '<S293>/CompTP' */

5293
static void AFCS_MODEL1_sind(real32_T *x)

5294
{

5295
  int8_T n;

5296
  real32_T c_x;

5297
  real32_T absx;

5298
  if (!((!rtIsInfF(*x)) && (!rtIsNaNF(*x)))) {

5299
    c_x = (rtNaNF);

5300
  } else {

5301
    c_x = AFCS_MODEL1_rt_remf_snf(*x, 360.0F);

5302
    absx = (real32_T)fabs(c_x);

5303
    if (absx > 180.0F) {

5304
      if (c_x > 0.0F) {

5305
        c_x -= 360.0F;

5306
      } else {

5307
        c_x += 360.0F;

5308
      }

5309


5310
      absx = (real32_T)fabs(c_x);

5311
    }

5312


5313
    if (absx <= 45.0F) {

5314
      c_x *= 0.0174532924F;

5315
      n = 0;

5316
    } else if (absx <= 135.0F) {

5317
      if (c_x > 0.0F) {

5318
        c_x = (c_x - 90.0F) * 0.0174532924F;

5319
        n = 1;

5320
      } else {

5321
        c_x = (c_x + 90.0F) * 0.0174532924F;

5322
        n = -1;

5323
      }

5324
    } else if (c_x > 0.0F) {

5325
      c_x = (c_x - 180.0F) * 0.0174532924F;

5326
      n = 2;

5327
    } else {

5328
      c_x = (c_x + 180.0F) * 0.0174532924F;

5329
      n = -2;

5330
    }

5331


5332
    if (n == 0) {

5333
      c_x = (real32_T)sin(c_x);

5334
    } else if (n == 1) {

5335
      c_x = (real32_T)cos(c_x);

5336
    } else if (n == -1) {

5337
      c_x = -(real32_T)cos(c_x);

5338
    } else {

5339
      c_x = -(real32_T)sin(c_x);

5340
    }

5341
  }

5342


5343
  *x = c_x;

5344
}

5345


5346
/* Function for MATLAB Function: '<S9>/MATLAB Function2' */

5347
static void AFCS_MODEL1_cosd_i(real_T *x)

5348
{

5349
  int8_T n;

5350
  static real_T b_x;

5351
  static real_T absx;

5352
  if (!((!rtIsInf(*x)) && (!rtIsNaN(*x)))) {

5353
    *x = (rtNaN);

5354
  } else {

5355
    b_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);

5356
    absx = fabs(b_x);

5357
    if (absx > 180.0) {

5358
      if (b_x > 0.0) {

5359
        b_x -= 360.0;

5360
      } else {

5361
        b_x += 360.0;

5362
      }

5363


5364
      absx = fabs(b_x);

5365
    }

5366


5367
    if (absx <= 45.0) {

5368
      b_x *= 0.017453292519943295;

5369
      n = 0;

5370
    } else if (absx <= 135.0) {

5371
      if (b_x > 0.0) {

5372
        b_x = (b_x - 90.0) * 0.017453292519943295;

5373
        n = 1;

5374
      } else {

5375
        b_x = (b_x + 90.0) * 0.017453292519943295;

5376
        n = -1;

5377
      }

5378
    } else if (b_x > 0.0) {

5379
      b_x = (b_x - 180.0) * 0.017453292519943295;

5380
      n = 2;

5381
    } else {

5382
      b_x = (b_x + 180.0) * 0.017453292519943295;

5383
      n = -2;

5384
    }

5385


5386
    if (n == 0) {

5387
      *x = cos(b_x);

5388
    } else if (n == 1) {

5389
      *x = -sin(b_x);

5390
    } else if (n == -1) {

5391
      *x = sin(b_x);

5392
    } else {

5393
      *x = -cos(b_x);

5394
    }

5395
  }

5396
}

5397


5398
/* Function for MATLAB Function: '<S9>/MATLAB Function2' */

5399
static void AFCS_MODEL1_sind_k(real_T *x)

5400
{

5401
  int8_T n;

5402
  static real_T c_x;

5403
  static real_T absx;

5404
  if (!((!rtIsInf(*x)) && (!rtIsNaN(*x)))) {

5405
    c_x = (rtNaN);

5406
  } else {

5407
    c_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);

5408
    absx = fabs(c_x);

5409
    if (absx > 180.0) {

5410
      if (c_x > 0.0) {

5411
        c_x -= 360.0;

5412
      } else {

5413
        c_x += 360.0;

5414
      }

5415


5416
      absx = fabs(c_x);

5417
    }

5418


5419
    if (absx <= 45.0) {

5420
      c_x *= 0.017453292519943295;

5421
      n = 0;

5422
    } else if (absx <= 135.0) {

5423
      if (c_x > 0.0) {

5424
        c_x = (c_x - 90.0) * 0.017453292519943295;

5425
        n = 1;

5426
      } else {

5427
        c_x = (c_x + 90.0) * 0.017453292519943295;

5428
        n = -1;

5429
      }

5430
    } else if (c_x > 0.0) {

5431
      c_x = (c_x - 180.0) * 0.017453292519943295;

5432
      n = 2;

5433
    } else {

5434
      c_x = (c_x + 180.0) * 0.017453292519943295;

5435
      n = -2;

5436
    }

5437


5438
    if (n == 0) {

5439
      c_x = sin(c_x);

5440
    } else if (n == 1) {

5441
      c_x = cos(c_x);

5442
    } else if (n == -1) {

5443
      c_x = -cos(c_x);

5444
    } else {

5445
      c_x = -sin(c_x);

5446
    }

5447
  }

5448


5449
  *x = c_x;

5450
}

5451


5452
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

5453
static void AFCS_MODEL1_PGT_circledot(real_T x, real_T y, real_T b_vx, real_T

5454
  b_vy, real_T r, real_T *x_dot, real_T *y_dot, real_T *fi)

5455
{

5456
  real_T thet;

5457


5458
  /* UNTITLED3 Summary of this function goes here */

5459
  /*    Detailed explanation goes here */

5460
  /*   YHT：求圆心坐标 */

5461
  /* x,y,vx,vy:航点的位置坐标和速度 */

5462
  /* r:圆的半径 */

5463
  /* flag=1，表示顺时针，flag=0,表示逆时针 */

5464
  if (b_vx >= 0.0) {

5465
    if (fabs(b_vy) < 1.0E-7) {

5466
      thet = 0.0;

5467
    } else {

5468
      thet = (atan(b_vy / b_vx) + 6.2831853071795862) / 6.2831853071795862;

5469
      if (fabs(thet - AFCS_MODEL1_rt_roundd_snf(thet)) <= 2.2204460492503131E-16

5470
          * fabs(thet)) {

5471
        thet = 0.0;

5472
      } else {

5473
        thet = (thet - floor(thet)) * 6.2831853071795862;

5474
      }

5475
    }

5476
  } else if (fabs(b_vy) < 1.0E-7) {

5477
    thet = 3.1415926535897931;

5478
  } else {

5479
    thet = ((atan(b_vy / b_vx) + 3.1415926535897931) + 6.2831853071795862) /

5480
      6.2831853071795862;

5481
    if (fabs(thet - AFCS_MODEL1_rt_roundd_snf(thet)) <= 2.2204460492503131E-16 *

5482
        fabs(thet)) {

5483
      thet = 0.0;

5484
    } else {

5485
      thet = (thet - floor(thet)) * 6.2831853071795862;

5486
    }

5487


5488
    /* thet是切点处速度方向与北向的夹角，顺时针为正，0~360，yht */

5489
  }

5490


5491
  *x_dot = x - cos(thet + 1.5707963267948966) * r;

5492
  *y_dot = y - sin(thet + 1.5707963267948966) * r;

5493
  *fi = ((thet + 1.5707963267948966) + 6.2831853071795862) / 6.2831853071795862;

5494
  if (fabs(*fi - AFCS_MODEL1_rt_roundd_snf(*fi)) <= 2.2204460492503131E-16 *

5495
      fabs(*fi)) {

5496
    *fi = 0.0;

5497
  } else {

5498
    *fi = (*fi - floor(*fi)) * 6.2831853071795862;

5499
  }

5500
}

5501


5502
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

5503
static void AFCS_MODEL1_PGT_Dubins_2D(real_T x1, real_T b_y1, real_T v1x, real_T

5504
  v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T *xq1,

5505
  real_T *yq1, real_T *fis, real_T *fi1, real_T *xq2, real_T *yq2, real_T *fif,

5506
  real_T *fi2, real_T *L, real_T *A, real_T *B, real_T *C, real_T *x0_dot,

5507
  real_T *y0_dot, real_T *tx_dot, real_T *ty_dot)

5508
{

5509
  static real_T fi2_rw_in_sum;

5510
  static real_T b_x0_dot;

5511
  static real_T b_y0_dot;

5512
  static real_T b_fis;

5513
  static real_T b_tx_dot;

5514
  static real_T b_ty_dot;

5515
  static real_T b_fif;

5516
  static real_T fi1_rw_out_sum;

5517
  static real_T xq12_rw_out;

5518
  static real_T yq12_rw_out;

5519
  static real_T xq12_rw_in;

5520
  static real_T yq12_rw_in;

5521
  static real_T L12_rw_out;

5522
  static real_T c;

5523
  static real_T afa;

5524


5525
  /* UNTITLED 此处显示有关此函数的摘要 */

5526
  /*    此处显示详细说明 */

5527
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

5528
  /* xq1,yq1为圆1上切点的位置 */

5529
  /* fis,fi1为圆1上起点和切出点的位置 */

5530
  /* Lamda1表示圆1的方向，Lamda1=-1表示顺时针，Lamda1=1表示逆时针 */

5531
  /* xq2,yq2为圆2上切点的位置 */

5532
  /* fif,fi2为圆2上切入点和终点的位置 */

5533
  /* Lamda2表示圆2的方向，Lamda2=-1表示顺时针，Lamda2=1表示逆时针 */

5534
  /*  L切线段长度。 */

5535
  /*  A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */

5536
  /* x0_dot,y0_dot,tx_dot,ty_dot为圆1和圆2的圆心坐标 */

5537
  /* x1,y1,v1x,v1y,x2,y2,v2x,v2y,r_min分别对应起始点位置，速度，终止点位置，速度 */

5538
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

5539
  /* %求圆心坐标 */

5540
  if (fabs(v1x) + fabs(v1y) < 1.0E-7) {

5541
    *x0_dot = x1;

5542
    *y0_dot = b_y1;

5543
    *fis = 0.0;

5544
  } else {

5545
    AFCS_MODEL1_PGT_circledot(x1, b_y1, v1x, v1y, r_min, &b_x0_dot, &b_y0_dot,

5546
      &b_fis);

5547
    *x0_dot = b_x0_dot;

5548
    *y0_dot = b_y0_dot;

5549
    *fis = b_fis;

5550


5551
    /* 起始圆圆心坐标，fis是圆1上的起点角度 */

5552
  }

5553


5554
  if (fabs(v2x) + fabs(v2y) < 1.0E-7) {

5555
    *tx_dot = x2;

5556
    *ty_dot = y2;

5557
    *fif = 0.0;

5558
  } else {

5559
    AFCS_MODEL1_PGT_circledot(x2, y2, v2x, v2y, r_min, &b_tx_dot, &b_ty_dot,

5560
      &b_fif);

5561
    *tx_dot = b_tx_dot;

5562
    *ty_dot = b_ty_dot;

5563
    *fif = b_fif;

5564


5565
    /* 目标圆圆心坐标 */

5566
  }

5567


5568
  /* %求路径长度及相应坐标 */

5569
  /* UNTITLED2 Summary of this function goes here */

5570
  /* 计算两圆之间的dubins路径长度，返回长度值和圆2的切入点位置 */

5571
  /* 输入： */

5572
  /* fis1: */

5573
  /* x1,y1,r1:圆1的圆心坐标及半径 */

5574
  /* flag1:圆1的转动方向标志 */

5575
  /* x2,y2,r2:圆2的圆心坐标及半径 */

5576
  /* flag2:圆2的转动方向标志 */

5577
  /* 输出： */

5578
  /*  */

5579
  /*    Detailed explanation goes here */

5580
  /* UNTITLED2 Summary of this function goes here */

5581
  /*    Detailed explanation goes here */

5582
  /* flag1=0,表示圆1在切线右边，flag1=1，表示圆1在切线左边； */

5583
  /* flag2=0，表示外切线，flag2=1，表示内切线。 */

5584
  /*  A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */

5585
  /*  L切线段长度。 */

5586
  /* fi1表示圆1上切点的位置，fi2表示圆2上切点的位置。 */

5587
  /* 输入： */

5588
  /* x1,y1,r1:圆1的圆心坐标及半径 */

5589
  /* flag1:圆1的转动方向标志 */

5590
  /* x2,y2,r2:圆2的圆心坐标及半径 */

5591
  /* flag2:圆2的转动方向标志 */

5592
  if ((*x0_dot == *tx_dot) && (*y0_dot == *ty_dot)) {

5593
    /* 如果圆1和圆2重合 */

5594
    xq12_rw_out = 0.0;

5595
    yq12_rw_out = 0.0;

5596
    *fi1 = 0.0;

5597
    xq12_rw_in = 0.0;

5598
    yq12_rw_in = 0.0;

5599
    *fi2 = 0.0;

5600
    L12_rw_out = 1.0E+7;

5601
    *A = 1.0;

5602
    *B = 1.0;

5603
    *C = 1.0;

5604
  } else {

5605
    c = *x0_dot - *tx_dot;

5606
    *fi1 = *y0_dot - *ty_dot;

5607
    c = sqrt(c * c + *fi1 * *fi1);

5608


5609
    /* 两圆的距离 */

5610
    /* 外切线存在条件d_w<0; */

5611
    /* 内切线存在条件d_n<0; */

5612
    /*  圆在切线的右侧 */

5613
    if (fabs(r_min - r_min) - c < 0.0) {

5614
      /* 外切 */

5615
      afa = asin((r_min - r_min) / c);

5616
      *fi2 = AFCS_MODEL1_rt_atan2d_snf(*ty_dot - *y0_dot, *tx_dot - *x0_dot);

5617
      *fi1 = (((afa + *fi2) + 1.5707963267948966) + 6.2831853071795862) /

5618
        6.2831853071795862;

5619
      if (fabs(*fi1 - AFCS_MODEL1_rt_roundd_snf(*fi1)) <= 2.2204460492503131E-16

5620
          * fabs(*fi1)) {

5621
        *fi1 = 0.0;

5622
      } else {

5623
        *fi1 = (*fi1 - floor(*fi1)) * 6.2831853071795862;

5624
      }

5625


5626
      *fi2 = (((afa + *fi2) + 1.5707963267948966) + 6.2831853071795862) /

5627
        6.2831853071795862;

5628
      if (fabs(*fi2 - AFCS_MODEL1_rt_roundd_snf(*fi2)) <= 2.2204460492503131E-16

5629
          * fabs(*fi2)) {

5630
        *fi2 = 0.0;

5631
      } else {

5632
        *fi2 = (*fi2 - floor(*fi2)) * 6.2831853071795862;

5633
      }

5634


5635
      xq12_rw_out = r_min * cos(*fi1) + *x0_dot;

5636
      yq12_rw_out = r_min * sin(*fi1) + *y0_dot;

5637
      xq12_rw_in = r_min * cos(*fi2) + *tx_dot;

5638
      yq12_rw_in = r_min * sin(*fi2) + *ty_dot;

5639
      L12_rw_out = xq12_rw_out - xq12_rw_in;

5640
      fi1_rw_out_sum = yq12_rw_out - yq12_rw_in;

5641
      L12_rw_out = sqrt(L12_rw_out * L12_rw_out + fi1_rw_out_sum *

5642
                        fi1_rw_out_sum);

5643
    } else {

5644
      *fi1 = 0.0;

5645
      *fi2 = 0.0;

5646
      xq12_rw_out = 0.0;

5647
      yq12_rw_out = 0.0;

5648
      xq12_rw_in = 0.0;

5649
      yq12_rw_in = 0.0;

5650
      L12_rw_out = 1.0E+7;

5651
    }

5652


5653
    *A = yq12_rw_in - yq12_rw_out;

5654
    *B = xq12_rw_out - xq12_rw_in;

5655
    *C = xq12_rw_in * yq12_rw_out - xq12_rw_out * yq12_rw_in;

5656
  }

5657


5658
  /* 上面的结果依次是：圆1的切线xy坐标、角度，圆2的xy坐标、角度，两圆的距离，切线方程的A,B,C三个系数 */

5659
  /* 圆2切入点的角度,与向的夹角 */

5660
  fi1_rw_out_sum = ((6.2831853071795862 + *fis) - *fi1) / 6.2831853071795862;

5661
  if (fabs(fi1_rw_out_sum - AFCS_MODEL1_rt_roundd_snf(fi1_rw_out_sum)) <=

5662
      2.2204460492503131E-16 * fabs(fi1_rw_out_sum)) {

5663
    fi1_rw_out_sum = 0.0;

5664
  } else {

5665
    fi1_rw_out_sum = (fi1_rw_out_sum - floor(fi1_rw_out_sum)) *

5666
      6.2831853071795862;

5667
  }

5668


5669
  if (fabs(*fis - *fi1) < 1.0E-7) {

5670
    fi1_rw_out_sum = 0.0;

5671


5672
    /* 这块暂时没看懂 */

5673
  }

5674


5675
  *xq1 = xq12_rw_out;

5676
  *yq1 = yq12_rw_out;

5677
  *xq2 = xq12_rw_in;

5678
  *yq2 = yq12_rw_in;

5679
  fi2_rw_in_sum = ((6.2831853071795862 + *fi2) - *fif) / 6.2831853071795862;

5680
  if (fabs(fi2_rw_in_sum - AFCS_MODEL1_rt_roundd_snf(fi2_rw_in_sum)) <=

5681
      2.2204460492503131E-16 * fabs(fi2_rw_in_sum)) {

5682
    fi2_rw_in_sum = 0.0;

5683
  } else {

5684
    fi2_rw_in_sum = (fi2_rw_in_sum - floor(fi2_rw_in_sum)) * 6.2831853071795862;

5685
  }

5686


5687
  if (fabs(*fi2 - *fif) < 1.0E-7) {

5688
    fi2_rw_in_sum = 0.0;

5689
  }

5690


5691
  *L = (fi1_rw_out_sum * r_min + L12_rw_out) + fi2_rw_in_sum * r_min;

5692
  if (fabs(v1x) + fabs(v1y) < 1.0E-7) {

5693
    *xq1 = x1;

5694
    *yq1 = b_y1;

5695
    *fi1 = 0.0;

5696
  }

5697


5698
  if (fabs(v2x) + fabs(v2y) < 1.0E-7) {

5699
    *xq2 = x2;

5700
    *yq2 = y2;

5701
    *fi2 = 0.0;

5702
  }

5703
}

5704


5705
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

5706
static void AFCS_MODEL1_PGT_circledot_b(real_T x, real_T y, real_T b_vx, real_T

5707
  b_vy, real_T r, real_T *x_dot, real_T *y_dot, real_T *fi)

5708
{

5709
  real_T thet;

5710


5711
  /* UNTITLED3 Summary of this function goes here */

5712
  /*    Detailed explanation goes here */

5713
  /*   YHT：求圆心坐标 */

5714
  /* x,y,vx,vy:航点的位置坐标和速度 */

5715
  /* r:圆的半径 */

5716
  /* flag=1，表示顺时针，flag=0,表示逆时针 */

5717
  if (b_vx >= 0.0) {

5718
    if (fabs(b_vy) < 1.0E-7) {

5719
      thet = 0.0;

5720
    } else {

5721
      thet = (atan(b_vy / b_vx) + 6.2831853071795862) / 6.2831853071795862;

5722
      if (fabs(thet - AFCS_MODEL1_rt_roundd_snf(thet)) <= 2.2204460492503131E-16

5723
          * fabs(thet)) {

5724
        thet = 0.0;

5725
      } else {

5726
        thet = (thet - floor(thet)) * 6.2831853071795862;

5727
      }

5728
    }

5729
  } else if (fabs(b_vy) < 1.0E-7) {

5730
    thet = 3.1415926535897931;

5731
  } else {

5732
    thet = ((atan(b_vy / b_vx) + 3.1415926535897931) + 6.2831853071795862) /

5733
      6.2831853071795862;

5734
    if (fabs(thet - AFCS_MODEL1_rt_roundd_snf(thet)) <= 2.2204460492503131E-16 *

5735
        fabs(thet)) {

5736
      thet = 0.0;

5737
    } else {

5738
      thet = (thet - floor(thet)) * 6.2831853071795862;

5739
    }

5740


5741
    /* thet是切点处速度方向与北向的夹角，顺时针为正，0~360，yht */

5742
  }

5743


5744
  *x_dot = x - cos(thet - 1.5707963267948966) * r;

5745
  *y_dot = y - sin(thet - 1.5707963267948966) * r;

5746
  *fi = ((thet - 1.5707963267948966) + 6.2831853071795862) / 6.2831853071795862;

5747
  if (fabs(*fi - AFCS_MODEL1_rt_roundd_snf(*fi)) <= 2.2204460492503131E-16 *

5748
      fabs(*fi)) {

5749
    *fi = 0.0;

5750
  } else {

5751
    *fi = (*fi - floor(*fi)) * 6.2831853071795862;

5752
  }

5753


5754
  /* 此值是什么意思？ */

5755
}

5756


5757
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

5758
static void AFCS_MODEL1_PGT_Dubins_2D_d(real_T x1, real_T b_y1, real_T v1x,

5759
  real_T v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T

5760
  *xq1, real_T *yq1, real_T *fis, real_T *fi1, real_T *xq2, real_T *yq2, real_T *

5761
  fif, real_T *fi2, real_T *L, real_T *A, real_T *B, real_T *C, real_T *x0_dot,

5762
  real_T *y0_dot, real_T *tx_dot, real_T *ty_dot)

5763
{

5764
  static real_T fi2_rw_in_sum;

5765
  static real_T b_x0_dot;

5766
  static real_T b_y0_dot;

5767
  static real_T b_fis;

5768
  static real_T b_tx_dot;

5769
  static real_T b_ty_dot;

5770
  static real_T b_fif;

5771
  static real_T fi1_rw_out_sum;

5772
  static real_T xq12_rw_out;

5773
  static real_T yq12_rw_out;

5774
  static real_T xq12_rw_in;

5775
  static real_T yq12_rw_in;

5776
  static real_T L12_rw_out;

5777
  static real_T c;

5778
  static real_T afa;

5779


5780
  /* UNTITLED 此处显示有关此函数的摘要 */

5781
  /*    此处显示详细说明 */

5782
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

5783
  /* xq1,yq1为圆1上切点的位置 */

5784
  /* fis,fi1为圆1上起点和切出点的位置 */

5785
  /* Lamda1表示圆1的方向，Lamda1=-1表示顺时针，Lamda1=1表示逆时针 */

5786
  /* xq2,yq2为圆2上切点的位置 */

5787
  /* fif,fi2为圆2上切入点和终点的位置 */

5788
  /* Lamda2表示圆2的方向，Lamda2=-1表示顺时针，Lamda2=1表示逆时针 */

5789
  /*  L切线段长度。 */

5790
  /*  A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */

5791
  /* x0_dot,y0_dot,tx_dot,ty_dot为圆1和圆2的圆心坐标 */

5792
  /* x1,y1,v1x,v1y,x2,y2,v2x,v2y,r_min分别对应起始点位置，速度，终止点位置，速度 */

5793
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

5794
  /* %求圆心坐标 */

5795
  if (fabs(v1x) + fabs(v1y) < 1.0E-7) {

5796
    *x0_dot = x1;

5797
    *y0_dot = b_y1;

5798
    *fis = 0.0;

5799
  } else {

5800
    AFCS_MODEL1_PGT_circledot(x1, b_y1, v1x, v1y, r_min, &b_x0_dot, &b_y0_dot,

5801
      &b_fis);

5802
    *x0_dot = b_x0_dot;

5803
    *y0_dot = b_y0_dot;

5804
    *fis = b_fis;

5805


5806
    /* 起始圆圆心坐标，fis是圆1上的起点角度 */

5807
  }

5808


5809
  if (fabs(v2x) + fabs(v2y) < 1.0E-7) {

5810
    *tx_dot = x2;

5811
    *ty_dot = y2;

5812
    *fif = 0.0;

5813
  } else {

5814
    AFCS_MODEL1_PGT_circledot_b(x2, y2, v2x, v2y, r_min, &b_tx_dot, &b_ty_dot,

5815
      &b_fif);

5816
    *tx_dot = b_tx_dot;

5817
    *ty_dot = b_ty_dot;

5818
    *fif = b_fif;

5819


5820
    /* 目标圆圆心坐标 */

5821
  }

5822


5823
  /* %求路径长度及相应坐标 */

5824
  /* UNTITLED2 Summary of this function goes here */

5825
  /* 计算两圆之间的dubins路径长度，返回长度值和圆2的切入点位置 */

5826
  /* 输入： */

5827
  /* fis1: */

5828
  /* x1,y1,r1:圆1的圆心坐标及半径 */

5829
  /* flag1:圆1的转动方向标志 */

5830
  /* x2,y2,r2:圆2的圆心坐标及半径 */

5831
  /* flag2:圆2的转动方向标志 */

5832
  /* 输出： */

5833
  /*  */

5834
  /*    Detailed explanation goes here */

5835
  /* UNTITLED2 Summary of this function goes here */

5836
  /*    Detailed explanation goes here */

5837
  /* flag1=0,表示圆1在切线右边，flag1=1，表示圆1在切线左边； */

5838
  /* flag2=0，表示外切线，flag2=1，表示内切线。 */

5839
  /*  A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */

5840
  /*  L切线段长度。 */

5841
  /* fi1表示圆1上切点的位置，fi2表示圆2上切点的位置。 */

5842
  /* 输入： */

5843
  /* x1,y1,r1:圆1的圆心坐标及半径 */

5844
  /* flag1:圆1的转动方向标志 */

5845
  /* x2,y2,r2:圆2的圆心坐标及半径 */

5846
  /* flag2:圆2的转动方向标志 */

5847
  if ((*x0_dot == *tx_dot) && (*y0_dot == *ty_dot)) {

5848
    /* 如果圆1和圆2重合 */

5849
    xq12_rw_out = 0.0;

5850
    yq12_rw_out = 0.0;

5851
    *fi1 = 0.0;

5852
    xq12_rw_in = 0.0;

5853
    yq12_rw_in = 0.0;

5854
    *fi2 = 0.0;

5855
    L12_rw_out = 1.0E+7;

5856
    *A = 1.0;

5857
    *B = 1.0;

5858
    *C = 1.0;

5859
  } else {

5860
    c = *x0_dot - *tx_dot;

5861
    *fi1 = *y0_dot - *ty_dot;

5862
    c = sqrt(c * c + *fi1 * *fi1);

5863


5864
    /* 两圆的距离 */

5865
    /* 外切线存在条件d_w<0; */

5866
    /* 内切线存在条件d_n<0; */

5867
    /*  圆在切线的右侧 */

5868
    if (fabs(r_min + r_min) - c < 0.0) {

5869
      /* 内切 */

5870
      afa = acos((r_min + r_min) / c);

5871
      *fi2 = AFCS_MODEL1_rt_atan2d_snf(*ty_dot - *y0_dot, *tx_dot - *x0_dot);

5872
      *fi1 = ((afa + *fi2) + 6.2831853071795862) / 6.2831853071795862;

5873
      if (fabs(*fi1 - AFCS_MODEL1_rt_roundd_snf(*fi1)) <= 2.2204460492503131E-16

5874
          * fabs(*fi1)) {

5875
        *fi1 = 0.0;

5876
      } else {

5877
        *fi1 = (*fi1 - floor(*fi1)) * 6.2831853071795862;

5878
      }

5879


5880
      *fi2 = (((afa + *fi2) + 3.1415926535897931) + 6.2831853071795862) /

5881
        6.2831853071795862;

5882
      if (fabs(*fi2 - AFCS_MODEL1_rt_roundd_snf(*fi2)) <= 2.2204460492503131E-16

5883
          * fabs(*fi2)) {

5884
        *fi2 = 0.0;

5885
      } else {

5886
        *fi2 = (*fi2 - floor(*fi2)) * 6.2831853071795862;

5887
      }

5888


5889
      xq12_rw_out = r_min * cos(*fi1) + *x0_dot;

5890
      yq12_rw_out = r_min * sin(*fi1) + *y0_dot;

5891
      xq12_rw_in = r_min * cos(*fi2) + *tx_dot;

5892
      yq12_rw_in = r_min * sin(*fi2) + *ty_dot;

5893
      L12_rw_out = xq12_rw_out - xq12_rw_in;

5894
      fi1_rw_out_sum = yq12_rw_out - yq12_rw_in;

5895
      L12_rw_out = sqrt(L12_rw_out * L12_rw_out + fi1_rw_out_sum *

5896
                        fi1_rw_out_sum);

5897
    } else {

5898
      *fi1 = 0.0;

5899
      *fi2 = 0.0;

5900
      xq12_rw_out = 0.0;

5901
      yq12_rw_out = 0.0;

5902
      xq12_rw_in = 0.0;

5903
      yq12_rw_in = 0.0;

5904
      L12_rw_out = 1.0E+7;

5905
    }

5906


5907
    *A = yq12_rw_in - yq12_rw_out;

5908
    *B = xq12_rw_out - xq12_rw_in;

5909
    *C = xq12_rw_in * yq12_rw_out - xq12_rw_out * yq12_rw_in;

5910
  }

5911


5912
  /* 上面的结果依次是：圆1的切线xy坐标、角度，圆2的xy坐标、角度，两圆的距离，切线方程的A,B,C三个系数 */

5913
  /* 圆2切入点的角度,与向的夹角 */

5914
  fi1_rw_out_sum = ((6.2831853071795862 + *fis) - *fi1) / 6.2831853071795862;

5915
  if (fabs(fi1_rw_out_sum - AFCS_MODEL1_rt_roundd_snf(fi1_rw_out_sum)) <=

5916
      2.2204460492503131E-16 * fabs(fi1_rw_out_sum)) {

5917
    fi1_rw_out_sum = 0.0;

5918
  } else {

5919
    fi1_rw_out_sum = (fi1_rw_out_sum - floor(fi1_rw_out_sum)) *

5920
      6.2831853071795862;

5921
  }

5922


5923
  if (fabs(*fis - *fi1) < 1.0E-7) {

5924
    fi1_rw_out_sum = 0.0;

5925


5926
    /* 这块暂时没看懂 */

5927
  }

5928


5929
  *xq1 = xq12_rw_out;

5930
  *yq1 = yq12_rw_out;

5931
  *xq2 = xq12_rw_in;

5932
  *yq2 = yq12_rw_in;

5933
  fi2_rw_in_sum = ((6.2831853071795862 + *fif) - *fi2) / 6.2831853071795862;

5934
  if (fabs(fi2_rw_in_sum - AFCS_MODEL1_rt_roundd_snf(fi2_rw_in_sum)) <=

5935
      2.2204460492503131E-16 * fabs(fi2_rw_in_sum)) {

5936
    fi2_rw_in_sum = 0.0;

5937
  } else {

5938
    fi2_rw_in_sum = (fi2_rw_in_sum - floor(fi2_rw_in_sum)) * 6.2831853071795862;

5939
  }

5940


5941
  if (fabs(*fi2 - *fif) < 1.0E-7) {

5942
    fi2_rw_in_sum = 0.0;

5943
  }

5944


5945
  *L = (fi1_rw_out_sum * r_min + L12_rw_out) + fi2_rw_in_sum * r_min;

5946
  if (fabs(v1x) + fabs(v1y) < 1.0E-7) {

5947
    *xq1 = x1;

5948
    *yq1 = b_y1;

5949
    *fi1 = 0.0;

5950
  }

5951


5952
  if (fabs(v2x) + fabs(v2y) < 1.0E-7) {

5953
    *xq2 = x2;

5954
    *yq2 = y2;

5955
    *fi2 = 0.0;

5956
  }

5957
}

5958


5959
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

5960
static void AFCS_MODEL1_PGT_Dubins_2Dmin(real_T x1, real_T b_y1, real_T v1x,

5961
  real_T v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T

5962
  *xq1, real_T *yq1, real_T *fis, real_T *fi1, real_T *Lamda1, real_T *xq2,

5963
  real_T *yq2, real_T *fif, real_T *fi2, real_T *Lamda2, real_T *L, real_T

5964
  *x0_dot, real_T *y0_dot, real_T *tx_dot, real_T *ty_dot)

5965
{

5966
  static real_T xq1_1;

5967
  static real_T yq1_1;

5968
  static real_T fis_1;

5969
  static real_T fi1_1;

5970
  static real_T xq2_1;

5971
  static real_T yq2_1;

5972
  static real_T fif_1;

5973
  static real_T fi2_1;

5974
  static real_T L_1;

5975
  static real_T unusedU0;

5976
  static real_T unusedU1;

5977
  static real_T unusedU2;

5978
  static real_T x0_dot_1;

5979
  static real_T y0_dot_1;

5980
  static real_T tx_dot_1;

5981
  static real_T ty_dot_1;

5982
  static real_T xq1_2;

5983
  static real_T yq1_2;

5984
  static real_T fis_2;

5985
  static real_T fi1_2;

5986
  static real_T xq2_2;

5987
  static real_T yq2_2;

5988
  static real_T fif_2;

5989
  static real_T fi2_2;

5990
  static real_T L_2;

5991
  static real_T unusedU3;

5992
  static real_T unusedU4;

5993
  static real_T unusedU5;

5994
  static real_T x0_dot_2;

5995
  static real_T y0_dot_2;

5996
  static real_T tx_dot_2;

5997
  static real_T ty_dot_2;

5998
  static real_T xq1_3;

5999
  static real_T yq1_3;

6000
  static real_T fis_3;

6001
  static real_T fi1_3;

6002
  static real_T xq2_3;

6003
  static real_T yq2_3;

6004
  static real_T fif_3;

6005
  static real_T fi2_3;

6006
  static real_T L_3;

6007
  static real_T x0_dot_3;

6008
  static real_T y0_dot_3;

6009
  static real_T tx_dot_3;

6010
  static real_T ty_dot_3;

6011
  static real_T xq1_4;

6012
  static real_T yq1_4;

6013
  static real_T fis_4;

6014
  static real_T fi1_4;

6015
  static real_T xq2_4;

6016
  static real_T yq2_4;

6017
  static real_T fif_4;

6018
  static real_T fi2_4;

6019
  static real_T L_4;

6020
  static real_T x0_dot_4;

6021
  static real_T y0_dot_4;

6022
  static real_T tx_dot_4;

6023
  static real_T ty_dot_4;

6024
  static real_T fi2_rw_in_sum;

6025
  static real_T b_x0_dot;

6026
  static real_T b_y0_dot;

6027
  static real_T b_fis;

6028
  static real_T b_tx_dot;

6029
  static real_T b_ty_dot;

6030
  static real_T b_fif;

6031
  static real_T fi1_rw_out_sum;

6032
  static real_T xq12_rw_out;

6033
  static real_T yq12_rw_out;

6034
  static real_T fi12_rw_out;

6035
  static real_T xq12_rw_in;

6036
  static real_T yq12_rw_in;

6037
  static real_T fi12_rw_in;

6038
  static real_T L12_rw_out;

6039
  static real_T c;

6040
  static real_T afa;

6041
  static real_T belt;

6042
  static real_T a;

6043
  static real_T b_a;

6044
  static real_T c_a;

6045
  static real_T d_a;

6046
  static real_T b_fi2_rw_in_sum;

6047
  static real_T c_x0_dot;

6048
  static real_T c_y0_dot;

6049
  static real_T c_fis;

6050
  static real_T c_tx_dot;

6051
  static real_T c_ty_dot;

6052
  static real_T c_fif;

6053
  static real_T b_fi1_rw_out_sum;

6054
  static real_T b_xq12_rw_out;

6055
  static real_T b_yq12_rw_out;

6056
  static real_T b_fi12_rw_out;

6057
  static real_T b_xq12_rw_in;

6058
  static real_T b_yq12_rw_in;

6059
  static real_T b_fi12_rw_in;

6060
  static real_T b_L12_rw_out;

6061
  static real_T b_c;

6062
  static real_T b_afa;

6063
  static real_T b_belt;

6064
  static real_T e_a;

6065
  static real_T f_a;

6066
  static real_T g_a;

6067
  static real_T h_a;

6068


6069
  /* UNTITLED2 此处显示有关此函数的摘要 */

6070
  /*    此处显示详细说明 */

6071
  /* %1 */

6072
  AFCS_MODEL1_PGT_Dubins_2D(x1, b_y1, v1x, v1y, x2, y2, v2x, v2y, r_min, &xq1_1,

6073
    &yq1_1, &fis_1, &fi1_1, &xq2_1, &yq2_1, &fif_1, &fi2_1, &L_1, &unusedU0,

6074
    &unusedU1, &unusedU2, &x0_dot_1, &y0_dot_1, &tx_dot_1, &ty_dot_1);

6075
  *xq1 = xq1_1;

6076
  *yq1 = yq1_1;

6077
  *fis = fis_1;

6078
  *fi1 = fi1_1;

6079
  *Lamda1 = 1.0;

6080
  *xq2 = xq2_1;

6081
  *yq2 = yq2_1;

6082
  *fif = fif_1;

6083
  *fi2 = fi2_1;

6084
  *Lamda2 = 1.0;

6085
  *L = L_1;

6086
  *x0_dot = x0_dot_1;

6087
  *y0_dot = y0_dot_1;

6088
  *tx_dot = tx_dot_1;

6089
  *ty_dot = ty_dot_1;

6090


6091
  /* %2 */

6092
  AFCS_MODEL1_PGT_Dubins_2D_d(x1, b_y1, v1x, v1y, x2, y2, v2x, v2y, r_min,

6093
    &xq1_2, &yq1_2, &fis_2, &fi1_2, &xq2_2, &yq2_2, &fif_2, &fi2_2, &L_2,

6094
    &unusedU3, &unusedU4, &unusedU5, &x0_dot_2, &y0_dot_2, &tx_dot_2, &ty_dot_2);

6095
  if (L_2 <= L_1 + 1.0E-7) {

6096
    *xq1 = xq1_2;

6097
    *yq1 = yq1_2;

6098
    *fis = fis_2;

6099
    *fi1 = fi1_2;

6100
    *xq2 = xq2_2;

6101
    *yq2 = yq2_2;

6102
    *fif = fif_2;

6103
    *fi2 = fi2_2;

6104
    *Lamda2 = -1.0;

6105
    *L = L_2;

6106
    *x0_dot = x0_dot_2;

6107
    *y0_dot = y0_dot_2;

6108
    *tx_dot = tx_dot_2;

6109
    *ty_dot = ty_dot_2;

6110
  }

6111


6112
  /* %3 */

6113
  /* UNTITLED 此处显示有关此函数的摘要 */

6114
  /*    此处显示详细说明 */

6115
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

6116
  /* xq1,yq1为圆1上切点的位置 */

6117
  /* fis,fi1为圆1上起点和切出点的位置 */

6118
  /* Lamda1表示圆1的方向，Lamda1=-1表示顺时针，Lamda1=1表示逆时针 */

6119
  /* xq2,yq2为圆2上切点的位置 */

6120
  /* fif,fi2为圆2上切入点和终点的位置 */

6121
  /* Lamda2表示圆2的方向，Lamda2=-1表示顺时针，Lamda2=1表示逆时针 */

6122
  /*  L切线段长度。 */

6123
  /*  A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */

6124
  /* x0_dot,y0_dot,tx_dot,ty_dot为圆1和圆2的圆心坐标 */

6125
  /* x1,y1,v1x,v1y,x2,y2,v2x,v2y,r_min分别对应起始点位置，速度，终止点位置，速度 */

6126
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

6127
  /* %求圆心坐标 */

6128
  if (fabs(v1x) + fabs(v1y) < 1.0E-7) {

6129
    x0_dot_3 = x1;

6130
    y0_dot_3 = b_y1;

6131
    fis_3 = 0.0;

6132
  } else {

6133
    AFCS_MODEL1_PGT_circledot_b(x1, b_y1, v1x, v1y, r_min, &b_x0_dot, &b_y0_dot,

6134
      &b_fis);

6135
    x0_dot_3 = b_x0_dot;

6136
    y0_dot_3 = b_y0_dot;

6137
    fis_3 = b_fis;

6138


6139
    /* 起始圆圆心坐标，fis是圆1上的起点角度 */

6140
  }

6141


6142
  if (fabs(v2x) + fabs(v2y) < 1.0E-7) {

6143
    tx_dot_3 = x2;

6144
    ty_dot_3 = y2;

6145
    fif_3 = 0.0;

6146
  } else {

6147
    AFCS_MODEL1_PGT_circledot(x2, y2, v2x, v2y, r_min, &b_tx_dot, &b_ty_dot,

6148
      &b_fif);

6149
    tx_dot_3 = b_tx_dot;

6150
    ty_dot_3 = b_ty_dot;

6151
    fif_3 = b_fif;

6152


6153
    /* 目标圆圆心坐标 */

6154
  }

6155


6156
  /* 逆时针为1，顺时针为-1 */

6157
  /* %求路径长度及相应坐标 */

6158
  /* UNTITLED2 Summary of this function goes here */

6159
  /* 计算两圆之间的dubins路径长度，返回长度值和圆2的切入点位置 */

6160
  /* 输入： */

6161
  /* fis1: */

6162
  /* x1,y1,r1:圆1的圆心坐标及半径 */

6163
  /* flag1:圆1的转动方向标志 */

6164
  /* x2,y2,r2:圆2的圆心坐标及半径 */

6165
  /* flag2:圆2的转动方向标志 */

6166
  /* 输出： */

6167
  /*  */

6168
  /*    Detailed explanation goes here */

6169
  /* UNTITLED2 Summary of this function goes here */

6170
  /*    Detailed explanation goes here */

6171
  /* flag1=0,表示圆1在切线右边，flag1=1，表示圆1在切线左边； */

6172
  /* flag2=0，表示外切线，flag2=1，表示内切线。 */

6173
  /*  A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */

6174
  /*  L切线段长度。 */

6175
  /* fi1表示圆1上切点的位置，fi2表示圆2上切点的位置。 */

6176
  /* 输入： */

6177
  /* x1,y1,r1:圆1的圆心坐标及半径 */

6178
  /* flag1:圆1的转动方向标志 */

6179
  /* x2,y2,r2:圆2的圆心坐标及半径 */

6180
  /* flag2:圆2的转动方向标志 */

6181
  if ((x0_dot_3 == tx_dot_3) && (y0_dot_3 == ty_dot_3)) {

6182
    /* 如果圆1和圆2重合 */

6183
    xq12_rw_out = 0.0;

6184
    yq12_rw_out = 0.0;

6185
    fi12_rw_out = 0.0;

6186
    xq12_rw_in = 0.0;

6187
    yq12_rw_in = 0.0;

6188
    fi12_rw_in = 0.0;

6189
    L12_rw_out = 1.0E+7;

6190
  } else {

6191
    a = x0_dot_3 - tx_dot_3;

6192
    b_a = y0_dot_3 - ty_dot_3;

6193
    c = sqrt(a * a + b_a * b_a);

6194


6195
    /* 两圆的距离 */

6196
    /* 外切线存在条件d_w<0; */

6197
    /* 内切线存在条件d_n<0; */

6198
    /* 圆在切线的左侧 */

6199
    if (fabs(r_min + r_min) - c < 0.0) {

6200
      /* 内切 */

6201
      afa = acos((r_min + r_min) / c);

6202
      belt = AFCS_MODEL1_rt_atan2d_snf(ty_dot_3 - y0_dot_3, tx_dot_3 - x0_dot_3);

6203
      fi12_rw_out = (((-afa + belt) + 6.2831853071795862) + 6.2831853071795862) /

6204
        6.2831853071795862;

6205
      if (fabs(fi12_rw_out - AFCS_MODEL1_rt_roundd_snf(fi12_rw_out)) <=

6206
          2.2204460492503131E-16 * fabs(fi12_rw_out)) {

6207
        fi12_rw_out = 0.0;

6208
      } else {

6209
        fi12_rw_out = (fi12_rw_out - floor(fi12_rw_out)) * 6.2831853071795862;

6210
      }

6211


6212
      fi12_rw_in = (((-afa + belt) + 3.1415926535897931) + 6.2831853071795862) /

6213
        6.2831853071795862;

6214
      if (fabs(fi12_rw_in - AFCS_MODEL1_rt_roundd_snf(fi12_rw_in)) <=

6215
          2.2204460492503131E-16 * fabs(fi12_rw_in)) {

6216
        fi12_rw_in = 0.0;

6217
      } else {

6218
        fi12_rw_in = (fi12_rw_in - floor(fi12_rw_in)) * 6.2831853071795862;

6219
      }

6220


6221
      xq12_rw_out = r_min * cos(fi12_rw_out) + x0_dot_3;

6222
      yq12_rw_out = r_min * sin(fi12_rw_out) + y0_dot_3;

6223
      xq12_rw_in = r_min * cos(fi12_rw_in) + tx_dot_3;

6224
      yq12_rw_in = r_min * sin(fi12_rw_in) + ty_dot_3;

6225
      c_a = xq12_rw_out - xq12_rw_in;

6226
      d_a = yq12_rw_out - yq12_rw_in;

6227
      L12_rw_out = sqrt(c_a * c_a + d_a * d_a);

6228
    } else {

6229
      fi12_rw_out = 0.0;

6230
      fi12_rw_in = 0.0;

6231
      xq12_rw_out = 0.0;

6232
      yq12_rw_out = 0.0;

6233
      xq12_rw_in = 0.0;

6234
      yq12_rw_in = 0.0;

6235
      L12_rw_out = 1.0E+7;

6236
    }

6237
  }

6238


6239
  /* 上面的结果依次是：圆1的切线xy坐标、角度，圆2的xy坐标、角度，两圆的距离，切线方程的A,B,C三个系数 */

6240
  /* 圆2切入点的角度,与向的夹角 */

6241
  fi1_rw_out_sum = ((6.2831853071795862 + fi12_rw_out) - fis_3) /

6242
    6.2831853071795862;

6243
  if (fabs(fi1_rw_out_sum - AFCS_MODEL1_rt_roundd_snf(fi1_rw_out_sum)) <=

6244
      2.2204460492503131E-16 * fabs(fi1_rw_out_sum)) {

6245
    fi1_rw_out_sum = 0.0;

6246
  } else {

6247
    fi1_rw_out_sum = (fi1_rw_out_sum - floor(fi1_rw_out_sum)) *

6248
      6.2831853071795862;

6249
  }

6250


6251
  if (fabs(fis_3 - fi12_rw_out) < 1.0E-7) {

6252
    fi1_rw_out_sum = 0.0;

6253


6254
    /* 这块暂时没看懂 */

6255
  }

6256


6257
  xq1_3 = xq12_rw_out;

6258
  yq1_3 = yq12_rw_out;

6259
  fi1_3 = fi12_rw_out;

6260
  xq2_3 = xq12_rw_in;

6261
  yq2_3 = yq12_rw_in;

6262
  fi2_3 = fi12_rw_in;

6263
  fi2_rw_in_sum = ((6.2831853071795862 + fi12_rw_in) - fif_3) /

6264
    6.2831853071795862;

6265
  if (fabs(fi2_rw_in_sum - AFCS_MODEL1_rt_roundd_snf(fi2_rw_in_sum)) <=

6266
      2.2204460492503131E-16 * fabs(fi2_rw_in_sum)) {

6267
    fi2_rw_in_sum = 0.0;

6268
  } else {

6269
    fi2_rw_in_sum = (fi2_rw_in_sum - floor(fi2_rw_in_sum)) * 6.2831853071795862;

6270
  }

6271


6272
  if (fabs(fi12_rw_in - fif_3) < 1.0E-7) {

6273
    fi2_rw_in_sum = 0.0;

6274
  }

6275


6276
  L_3 = (fi1_rw_out_sum * r_min + L12_rw_out) + fi2_rw_in_sum * r_min;

6277
  if (fabs(v1x) + fabs(v1y) < 1.0E-7) {

6278
    xq1_3 = x1;

6279
    yq1_3 = b_y1;

6280
    fi1_3 = 0.0;

6281
  }

6282


6283
  if (fabs(v2x) + fabs(v2y) < 1.0E-7) {

6284
    xq2_3 = x2;

6285
    yq2_3 = y2;

6286
    fi2_3 = 0.0;

6287
  }

6288


6289
  if (L_3 <= *L + 1.0E-7) {

6290
    *xq1 = xq1_3;

6291
    *yq1 = yq1_3;

6292
    *fis = fis_3;

6293
    *fi1 = fi1_3;

6294
    *Lamda1 = -1.0;

6295
    *xq2 = xq2_3;

6296
    *yq2 = yq2_3;

6297
    *fif = fif_3;

6298
    *fi2 = fi2_3;

6299
    *Lamda2 = 1.0;

6300
    *L = L_3;

6301
    *x0_dot = x0_dot_3;

6302
    *y0_dot = y0_dot_3;

6303
    *tx_dot = tx_dot_3;

6304
    *ty_dot = ty_dot_3;

6305
  }

6306


6307
  /* %4 */

6308
  /* UNTITLED 此处显示有关此函数的摘要 */

6309
  /*    此处显示详细说明 */

6310
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

6311
  /* xq1,yq1为圆1上切点的位置 */

6312
  /* fis,fi1为圆1上起点和切出点的位置 */

6313
  /* Lamda1表示圆1的方向，Lamda1=-1表示顺时针，Lamda1=1表示逆时针 */

6314
  /* xq2,yq2为圆2上切点的位置 */

6315
  /* fif,fi2为圆2上切入点和终点的位置 */

6316
  /* Lamda2表示圆2的方向，Lamda2=-1表示顺时针，Lamda2=1表示逆时针 */

6317
  /*  L切线段长度。 */

6318
  /*  A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */

6319
  /* x0_dot,y0_dot,tx_dot,ty_dot为圆1和圆2的圆心坐标 */

6320
  /* x1,y1,v1x,v1y,x2,y2,v2x,v2y,r_min分别对应起始点位置，速度，终止点位置，速度 */

6321
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

6322
  /* %求圆心坐标 */

6323
  if (fabs(v1x) + fabs(v1y) < 1.0E-7) {

6324
    x0_dot_4 = x1;

6325
    y0_dot_4 = b_y1;

6326
    fis_4 = 0.0;

6327
  } else {

6328
    AFCS_MODEL1_PGT_circledot_b(x1, b_y1, v1x, v1y, r_min, &c_x0_dot, &c_y0_dot,

6329
      &c_fis);

6330
    x0_dot_4 = c_x0_dot;

6331
    y0_dot_4 = c_y0_dot;

6332
    fis_4 = c_fis;

6333


6334
    /* 起始圆圆心坐标，fis是圆1上的起点角度 */

6335
  }

6336


6337
  if (fabs(v2x) + fabs(v2y) < 1.0E-7) {

6338
    tx_dot_4 = x2;

6339
    ty_dot_4 = y2;

6340
    fif_4 = 0.0;

6341
  } else {

6342
    AFCS_MODEL1_PGT_circledot_b(x2, y2, v2x, v2y, r_min, &c_tx_dot, &c_ty_dot,

6343
      &c_fif);

6344
    tx_dot_4 = c_tx_dot;

6345
    ty_dot_4 = c_ty_dot;

6346
    fif_4 = c_fif;

6347


6348
    /* 目标圆圆心坐标 */

6349
  }

6350


6351
  /* 逆时针为1，顺时针为-1 */

6352
  /* %求路径长度及相应坐标 */

6353
  /* UNTITLED2 Summary of this function goes here */

6354
  /* 计算两圆之间的dubins路径长度，返回长度值和圆2的切入点位置 */

6355
  /* 输入： */

6356
  /* fis1: */

6357
  /* x1,y1,r1:圆1的圆心坐标及半径 */

6358
  /* flag1:圆1的转动方向标志 */

6359
  /* x2,y2,r2:圆2的圆心坐标及半径 */

6360
  /* flag2:圆2的转动方向标志 */

6361
  /* 输出： */

6362
  /*  */

6363
  /*    Detailed explanation goes here */

6364
  /* UNTITLED2 Summary of this function goes here */

6365
  /*    Detailed explanation goes here */

6366
  /* flag1=0,表示圆1在切线右边，flag1=1，表示圆1在切线左边； */

6367
  /* flag2=0，表示外切线，flag2=1，表示内切线。 */

6368
  /*  A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */

6369
  /*  L切线段长度。 */

6370
  /* fi1表示圆1上切点的位置，fi2表示圆2上切点的位置。 */

6371
  /* 输入： */

6372
  /* x1,y1,r1:圆1的圆心坐标及半径 */

6373
  /* flag1:圆1的转动方向标志 */

6374
  /* x2,y2,r2:圆2的圆心坐标及半径 */

6375
  /* flag2:圆2的转动方向标志 */

6376
  if ((x0_dot_4 == tx_dot_4) && (y0_dot_4 == ty_dot_4)) {

6377
    /* 如果圆1和圆2重合 */

6378
    b_xq12_rw_out = 0.0;

6379
    b_yq12_rw_out = 0.0;

6380
    b_fi12_rw_out = 0.0;

6381
    b_xq12_rw_in = 0.0;

6382
    b_yq12_rw_in = 0.0;

6383
    b_fi12_rw_in = 0.0;

6384
    b_L12_rw_out = 1.0E+7;

6385
  } else {

6386
    e_a = x0_dot_4 - tx_dot_4;

6387
    f_a = y0_dot_4 - ty_dot_4;

6388
    b_c = sqrt(e_a * e_a + f_a * f_a);

6389


6390
    /* 两圆的距离 */

6391
    /* 外切线存在条件d_w<0; */

6392
    /* 内切线存在条件d_n<0; */

6393
    /* 圆在切线的左侧 */

6394
    if (fabs(r_min - r_min) - b_c < 0.0) {

6395
      /* 外切 */

6396
      b_afa = asin((r_min - r_min) / b_c);

6397
      b_belt = AFCS_MODEL1_rt_atan2d_snf(ty_dot_4 - y0_dot_4, tx_dot_4 -

6398
        x0_dot_4);

6399
      b_fi12_rw_out = (((-b_afa + b_belt) + 4.71238898038469) +

6400
                       6.2831853071795862) / 6.2831853071795862;

6401
      if (fabs(b_fi12_rw_out - AFCS_MODEL1_rt_roundd_snf(b_fi12_rw_out)) <=

6402
          2.2204460492503131E-16 * fabs(b_fi12_rw_out)) {

6403
        b_fi12_rw_out = 0.0;

6404
      } else {

6405
        b_fi12_rw_out = (b_fi12_rw_out - floor(b_fi12_rw_out)) *

6406
          6.2831853071795862;

6407
      }

6408


6409
      b_fi12_rw_in = (((-b_afa + b_belt) + 4.71238898038469) +

6410
                      6.2831853071795862) / 6.2831853071795862;

6411
      if (fabs(b_fi12_rw_in - AFCS_MODEL1_rt_roundd_snf(b_fi12_rw_in)) <=

6412
          2.2204460492503131E-16 * fabs(b_fi12_rw_in)) {

6413
        b_fi12_rw_in = 0.0;

6414
      } else {

6415
        b_fi12_rw_in = (b_fi12_rw_in - floor(b_fi12_rw_in)) * 6.2831853071795862;

6416
      }

6417


6418
      b_xq12_rw_out = r_min * cos(b_fi12_rw_out) + x0_dot_4;

6419
      b_yq12_rw_out = r_min * sin(b_fi12_rw_out) + y0_dot_4;

6420
      b_xq12_rw_in = r_min * cos(b_fi12_rw_in) + tx_dot_4;

6421
      b_yq12_rw_in = r_min * sin(b_fi12_rw_in) + ty_dot_4;

6422
      g_a = b_xq12_rw_out - b_xq12_rw_in;

6423
      h_a = b_yq12_rw_out - b_yq12_rw_in;

6424
      b_L12_rw_out = sqrt(g_a * g_a + h_a * h_a);

6425
    } else {

6426
      b_fi12_rw_out = 0.0;

6427
      b_fi12_rw_in = 0.0;

6428
      b_xq12_rw_out = 0.0;

6429
      b_yq12_rw_out = 0.0;

6430
      b_xq12_rw_in = 0.0;

6431
      b_yq12_rw_in = 0.0;

6432
      b_L12_rw_out = 1.0E+7;

6433
    }

6434
  }

6435


6436
  /* 上面的结果依次是：圆1的切线xy坐标、角度，圆2的xy坐标、角度，两圆的距离，切线方程的A,B,C三个系数 */

6437
  /* 圆2切入点的角度,与向的夹角 */

6438
  b_fi1_rw_out_sum = ((6.2831853071795862 + b_fi12_rw_out) - fis_4) /

6439
    6.2831853071795862;

6440
  if (fabs(b_fi1_rw_out_sum - AFCS_MODEL1_rt_roundd_snf(b_fi1_rw_out_sum)) <=

6441
      2.2204460492503131E-16 * fabs(b_fi1_rw_out_sum)) {

6442
    b_fi1_rw_out_sum = 0.0;

6443
  } else {

6444
    b_fi1_rw_out_sum = (b_fi1_rw_out_sum - floor(b_fi1_rw_out_sum)) *

6445
      6.2831853071795862;

6446
  }

6447


6448
  if (fabs(fis_4 - b_fi12_rw_out) < 1.0E-7) {

6449
    b_fi1_rw_out_sum = 0.0;

6450


6451
    /* 这块暂时没看懂 */

6452
  }

6453


6454
  xq1_4 = b_xq12_rw_out;

6455
  yq1_4 = b_yq12_rw_out;

6456
  fi1_4 = b_fi12_rw_out;

6457
  xq2_4 = b_xq12_rw_in;

6458
  yq2_4 = b_yq12_rw_in;

6459
  fi2_4 = b_fi12_rw_in;

6460
  b_fi2_rw_in_sum = ((6.2831853071795862 + fif_4) - b_fi12_rw_in) /

6461
    6.2831853071795862;

6462
  if (fabs(b_fi2_rw_in_sum - AFCS_MODEL1_rt_roundd_snf(b_fi2_rw_in_sum)) <=

6463
      2.2204460492503131E-16 * fabs(b_fi2_rw_in_sum)) {

6464
    b_fi2_rw_in_sum = 0.0;

6465
  } else {

6466
    b_fi2_rw_in_sum = (b_fi2_rw_in_sum - floor(b_fi2_rw_in_sum)) *

6467
      6.2831853071795862;

6468
  }

6469


6470
  if (fabs(b_fi12_rw_in - fif_4) < 1.0E-7) {

6471
    b_fi2_rw_in_sum = 0.0;

6472
  }

6473


6474
  L_4 = (b_fi1_rw_out_sum * r_min + b_L12_rw_out) + b_fi2_rw_in_sum * r_min;

6475
  if (fabs(v1x) + fabs(v1y) < 1.0E-7) {

6476
    xq1_4 = x1;

6477
    yq1_4 = b_y1;

6478
    fi1_4 = 0.0;

6479
  }

6480


6481
  if (fabs(v2x) + fabs(v2y) < 1.0E-7) {

6482
    xq2_4 = x2;

6483
    yq2_4 = y2;

6484
    fi2_4 = 0.0;

6485
  }

6486


6487
  if (L_4 <= *L + 1.0E-7) {

6488
    *xq1 = xq1_4;

6489
    *yq1 = yq1_4;

6490
    *fis = fis_4;

6491
    *fi1 = fi1_4;

6492
    *Lamda1 = -1.0;

6493
    *xq2 = xq2_4;

6494
    *yq2 = yq2_4;

6495
    *fif = fif_4;

6496
    *fi2 = fi2_4;

6497
    *Lamda2 = -1.0;

6498
    *L = L_4;

6499
    *x0_dot = x0_dot_4;

6500
    *y0_dot = y0_dot_4;

6501
    *tx_dot = tx_dot_4;

6502
    *ty_dot = ty_dot_4;

6503
  }

6504


6505
  /*  */

6506
  /*  画圆 */

6507
  /*  % % if plotflag */

6508
  /*  % figure(2);subplot(2,2,1);plot([yq1_1,yq2_1],[xq1_1,xq2_1],'LineStyle','--','Marker','o','LineWidth',2,'color','r');hold on */

6509
  /*  % plot([y1,y2],[x1,x2],'LineStyle','none','Marker','o','LineWidth',4); axis equal */

6510
  /*  % rectangle('Position',[y0_dot_1- r_min,x0_dot_1-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','g') */

6511
  /*  % rectangle('Position',[ty_dot_1- r_min,tx_dot_1-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','b') */

6512
  /*  % %画图 */

6513
  /*  % figure(2);subplot(2,2,2);plot([yq1_2,yq2_2],[xq1_2,xq2_2],'LineStyle','--','Marker','o','LineWidth',2,'color','r');hold on */

6514
  /*  % plot([y1,y2],[x1,x2],'LineStyle','none','Marker','o','LineWidth',4); axis equal */

6515
  /*  % rectangle('Position',[y0_dot_2- r_min,x0_dot_2-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','g') */

6516
  /*  % rectangle('Position',[ty_dot_2- r_min,tx_dot_2-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','b') */

6517
  /*  % %画图 */

6518
  /*  % figure(2);subplot(2,2,3);plot([yq1_3,yq2_3],[xq1_3,xq2_3],'LineStyle','--','Marker','o','LineWidth',2,'color','r');hold on */

6519
  /*  % plot([y1,y2],[x1,x2],'LineStyle','none','Marker','o','LineWidth',4);  axis equal */

6520
  /*  % rectangle('Position',[y0_dot_3- r_min,x0_dot_3-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','g') */

6521
  /*  % rectangle('Position',[ty_dot_3- r_min,tx_dot_3-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','b') */

6522
  /*  % %画图 */

6523
  /*  % figure(2);subplot(2,2,4);plot([yq1_4,yq2_4],[xq1_3,xq2_4],'LineStyle','--','Marker','o','LineWidth',2,'color','r');hold on */

6524
  /*  % plot([y1,y2],[x1,x2],'LineStyle','none','Marker','o','LineWidth',4); axis equal */

6525
  /*  % rectangle('Position',[y0_dot_4- r_min,x0_dot_4-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','g') */

6526
  /*  % rectangle('Position',[ty_dot_4- r_min,tx_dot_4-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','b') */

6527
  /*  end */

6528
}

6529


6530
/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */

6531
static void AFCS_MODE_PGT_Guidance3D_Dubins(real_T xs, real_T ys, real_T hs,

6532
  real_T vxs, real_T vys, real_T xe, real_T ye, real_T he, real_T vxe, real_T

6533
  vye, real_T Rmin, real_T *R, real_T *Gama, real_T *xq1, real_T *yq1, real_T

6534
  *hq1, real_T *fis, real_T *Lamda1, real_T *xq2, real_T *yq2, real_T *hq2,

6535
  real_T *fi2, real_T *Lamda2, real_T *x0_dot, real_T *y0_dot, real_T *tx_dot,

6536
  real_T *ty_dot, real_T *fi1sum, real_T *fi2sum)

6537
{

6538
  static real_T fi1;

6539
  static real_T fif;

6540
  static real_T L;

6541
  int32_T count;

6542
  static real_T b_xq1;

6543
  static real_T b_yq1;

6544
  static real_T b_fis;

6545
  static real_T b_fi1;

6546
  static real_T b_Lamda1;

6547
  static real_T b_xq2;

6548
  static real_T b_yq2;

6549
  static real_T b_fif;

6550
  static real_T b_fi2;

6551
  static real_T b_Lamda2;

6552
  static real_T b_L;

6553
  static real_T b_x0_dot;

6554
  static real_T b_y0_dot;

6555
  static real_T b_tx_dot;

6556
  static real_T b_ty_dot;

6557
  static real_T c_xq1;

6558
  static real_T c_yq1;

6559
  static real_T c_fis;

6560
  static real_T c_fi1;

6561
  static real_T c_Lamda1;

6562
  static real_T c_xq2;

6563
  static real_T c_yq2;

6564
  static real_T c_fif;

6565
  static real_T c_fi2;

6566
  static real_T c_Lamda2;

6567
  static real_T c_L;

6568
  static real_T c_x0_dot;

6569
  static real_T c_y0_dot;

6570
  static real_T c_tx_dot;

6571
  static real_T c_ty_dot;

6572


6573
  /*  Rmax = sqrt((xs-xe)*(xs-xe) +(ys-ye)*(ys-ye)); */

6574
  /*  if Rmin > Rmax */

6575
  /*      Rmin = Rmax; */

6576
  /*  end */

6577
  AFCS_MODEL1_PGT_Dubins_2Dmin(xs, ys, vxs, vys, xe, ye, vxe, vye, Rmin, &b_xq1,

6578
    &b_yq1, &b_fis, &b_fi1, &b_Lamda1, &b_xq2, &b_yq2, &b_fif, &b_fi2, &b_Lamda2,

6579
    &b_L, &b_x0_dot, &b_y0_dot, &b_tx_dot, &b_ty_dot);

6580
  *xq1 = b_xq1;

6581
  *yq1 = b_yq1;

6582
  *fis = b_fis;

6583
  fi1 = b_fi1;

6584
  *Lamda1 = b_Lamda1;

6585
  *xq2 = b_xq2;

6586
  *yq2 = b_yq2;

6587
  fif = b_fif;

6588
  *fi2 = b_fi2;

6589
  *Lamda2 = b_Lamda2;

6590
  L = b_L;

6591
  *x0_dot = b_x0_dot;

6592
  *y0_dot = b_y0_dot;

6593
  *tx_dot = b_tx_dot;

6594
  *ty_dot = b_ty_dot;

6595


6596
  /* 2018/10/09修改 */

6597
  count = 0;

6598
  while ((L > 9.999999E+6) && (count < 10)) {

6599
    /* 如果计算的L超过最大值，重新计算 */

6600
    Rmin *= 0.9;

6601
    AFCS_MODEL1_PGT_Dubins_2Dmin(xs, ys, vxs, vys, xe, ye, vxe, vye, Rmin,

6602
      &c_xq1, &c_yq1, &c_fis, &c_fi1, &c_Lamda1, &c_xq2, &c_yq2, &c_fif, &c_fi2,

6603
      &c_Lamda2, &c_L, &c_x0_dot, &c_y0_dot, &c_tx_dot, &c_ty_dot);

6604
    *xq1 = c_xq1;

6605
    *yq1 = c_yq1;

6606
    *fis = c_fis;

6607
    fi1 = c_fi1;

6608
    *Lamda1 = c_Lamda1;

6609
    *xq2 = c_xq2;

6610
    *yq2 = c_yq2;

6611
    fif = c_fif;

6612
    *fi2 = c_fi2;

6613
    *Lamda2 = c_Lamda2;

6614
    L = c_L;

6615
    *x0_dot = c_x0_dot;

6616
    *y0_dot = c_y0_dot;

6617
    *tx_dot = c_tx_dot;

6618
    *ty_dot = c_ty_dot;

6619
    count++;

6620
  }

6621


6622
  *R = Rmin;

6623
  if (fabs(he - hs) <= L * 1.5574077246549023) {

6624
    *Gama = atan((he - hs) / L);

6625
  } else {

6626
    /*     %%二分法求R和Gama */

6627
    if (he >= hs) {

6628
      *Gama = 1.0;

6629
    } else {

6630
      *Gama = -1.0;

6631
    }

6632
  }

6633


6634
  /* %计算直线末端高度 */

6635
  /* %start */

6636
  if (*Lamda1 == 1.0) {

6637
    *fi1sum = ((6.2831853071795862 + *fis) - fi1) / 6.2831853071795862;

6638
    if (fabs(*fi1sum - AFCS_MODEL1_rt_roundd_snf(*fi1sum)) <=

6639
        2.2204460492503131E-16 * fabs(*fi1sum)) {

6640
      *fi1sum = 0.0;

6641
    } else {

6642
      *fi1sum = (*fi1sum - floor(*fi1sum)) * 6.2831853071795862;

6643
    }

6644
  } else {

6645
    *fi1sum = ((6.2831853071795862 + fi1) - *fis) / 6.2831853071795862;

6646
    if (fabs(*fi1sum - AFCS_MODEL1_rt_roundd_snf(*fi1sum)) <=

6647
        2.2204460492503131E-16 * fabs(*fi1sum)) {

6648
      *fi1sum = 0.0;

6649
    } else {

6650
      *fi1sum = (*fi1sum - floor(*fi1sum)) * 6.2831853071795862;

6651
    }

6652
  }

6653


6654
  if (fabs(fi1 - *fis) < 1.0E-7) {

6655
    *fi1sum = 0.0;

6656
  }

6657


6658
  *hq1 = hs;

6659


6660
  /* %end */

6661
  if (*Lamda2 == 1.0) {

6662
    *fi2sum = ((6.2831853071795862 + *fi2) - fif) / 6.2831853071795862;

6663
    if (fabs(*fi2sum - AFCS_MODEL1_rt_roundd_snf(*fi2sum)) <=

6664
        2.2204460492503131E-16 * fabs(*fi2sum)) {

6665
      *fi2sum = 0.0;

6666
    } else {

6667
      *fi2sum = (*fi2sum - floor(*fi2sum)) * 6.2831853071795862;

6668
    }

6669
  } else {

6670
    *fi2sum = ((6.2831853071795862 + fif) - *fi2) / 6.2831853071795862;

6671
    if (fabs(*fi2sum - AFCS_MODEL1_rt_roundd_snf(*fi2sum)) <=

6672
        2.2204460492503131E-16 * fabs(*fi2sum)) {

6673
      *fi2sum = 0.0;

6674
    } else {

6675
      *fi2sum = (*fi2sum - floor(*fi2sum)) * 6.2831853071795862;

6676
    }

6677
  }

6678


6679
  if (fabs(fif - *fi2) < 1.0E-7) {

6680
    *fi2sum = 0.0;

6681
  }

6682


6683
  *hq2 = he - *fi2sum * Rmin * tan(*Gama);

6684


6685
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

6686
}

6687


6688
/* System initialize for root system: '<Root>' */

6689
#define MDL_INITIALIZE_CONDITIONS

6690


6691
static void mdlInitializeConditions(SimStruct *S)

6692
{

6693
  if (ssIsFirstInitCond(S)) {

6694
    B_AFCS_MODEL1_T *_rtB;

6695
    X_AFCS_MODEL1_T *_rtX;

6696
    _rtX = ((X_AFCS_MODEL1_T *) ssGetContStates(S));

6697
    _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

6698


6699
    /* InitializeConditions for Memory: '<S433>/Memory10' */

6700
    ((real32_T *)ssGetDWork(S, 270))[0] = 0.0F;

6701


6702
    /* InitializeConditions for Memory: '<S438>/Memory1' */

6703
    ((real32_T *)ssGetDWork(S, 271))[0] = 0.0F;

6704


6705
    /* InitializeConditions for Memory: '<S437>/Memory1' */

6706
    ((real32_T *)ssGetDWork(S, 272))[0] = 0.0F;

6707


6708
    /* InitializeConditions for Memory: '<S436>/Memory1' */

6709
    ((real32_T *)ssGetDWork(S, 273))[0] = 0.0F;

6710


6711
    /* InitializeConditions for Memory: '<S435>/Memory1' */

6712
    ((real32_T *)ssGetDWork(S, 274))[0] = 0.0F;

6713


6714
    /* InitializeConditions for Memory: '<S434>/Memory1' */

6715
    ((real32_T *)ssGetDWork(S, 275))[0] = 0.0F;

6716


6717
    /* InitializeConditions for Memory: '<S439>/Memory1' */

6718
    ((real32_T *)ssGetDWork(S, 276))[0] = 0.0F;

6719


6720
    /* InitializeConditions for UnitDelay: '<S24>/Unit Delay3' */

6721
    ((int8_T *)ssGetDWork(S, 352))[0] = 0;

6722


6723
    /* InitializeConditions for UnitDelay: '<S7>/Unit Delay' */

6724
    ((int8_T *)ssGetDWork(S, 353))[0] = 1;

6725


6726
    /* InitializeConditions for UnitDelay: '<S7>/Unit Delay3' */

6727
    ((int8_T *)ssGetDWork(S, 354))[0] = 0;

6728


6729
    /* InitializeConditions for Memory: '<S9>/Memory1' */

6730
    ((boolean_T *)ssGetDWork(S, 612))[0] = false;

6731


6732
    /* InitializeConditions for Memory: '<S9>/Memory2' */

6733
    ((int8_T *)ssGetDWork(S, 504))[0] = 0;

6734


6735
    /* InitializeConditions for UnitDelay: '<S9>/Unit Delay' */

6736
    ((int8_T *)ssGetDWork(S, 355))[0] = 0;

6737


6738
    /* InitializeConditions for UnitDelay: '<S1>/Unit Delay1' */

6739
    ((int8_T *)ssGetDWork(S, 356))[0] = 0;

6740


6741
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay' */

6742
    ((int8_T *)ssGetDWork(S, 357))[0] = 0;

6743


6744
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay1' */

6745
    ((int8_T *)ssGetDWork(S, 358))[0] = 0;

6746


6747
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay2' */

6748
    ((int8_T *)ssGetDWork(S, 359))[0] = 0;

6749


6750
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay3' */

6751
    ((int8_T *)ssGetDWork(S, 360))[0] = 0;

6752


6753
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay4' */

6754
    ((int8_T *)ssGetDWork(S, 361))[0] = 0;

6755


6756
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay7' */

6757
    ((int8_T *)ssGetDWork(S, 362))[0] = 0;

6758


6759
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay5' */

6760
    ((int8_T *)ssGetDWork(S, 363))[0] = 0;

6761


6762
    /* InitializeConditions for UnitDelay: '<S32>/Unit Delay3' */

6763
    ((int8_T *)ssGetDWork(S, 364))[0] = 0;

6764


6765
    /* InitializeConditions for Delay: '<S310>/Delay' */

6766
    ((boolean_T *)ssGetDWork(S, 502))[0] = false;

6767


6768
    /* InitializeConditions for UnitDelay: '<S309>/Unit Delay6' */

6769
    ((int8_T *)ssGetDWork(S, 365))[0] = 0;

6770


6771
    /* InitializeConditions for UnitDelay: '<S309>/Unit Delay5' */

6772
    ((int8_T *)ssGetDWork(S, 366))[0] = 0;

6773


6774
    /* InitializeConditions for UnitDelay: '<S1>/Unit Delay' */

6775
    ((int8_T *)ssGetDWork(S, 367))[0] = 0;

6776


6777
    /* InitializeConditions for Delay: '<S431>/Delay' */

6778
    ((boolean_T *)ssGetDWork(S, 503))[0] = false;

6779


6780
    /* InitializeConditions for UnitDelay: '<S430>/Unit Delay6' */

6781
    ((int8_T *)ssGetDWork(S, 368))[0] = 0;

6782


6783
    /* InitializeConditions for UnitDelay: '<S336>/Unit Delay2' */

6784
    ((int8_T *)ssGetDWork(S, 369))[0] = 0;

6785


6786
    /* InitializeConditions for UnitDelay: '<S430>/Unit Delay5' */

6787
    ((int8_T *)ssGetDWork(S, 370))[0] = 0;

6788


6789
    /* InitializeConditions for Memory: '<S433>/Memory1' */

6790
    ((real32_T *)ssGetDWork(S, 277))[0] = 0.0F;

6791


6792
    /* InitializeConditions for Memory: '<S433>/Memory2' */

6793
    ((real32_T *)ssGetDWork(S, 278))[0] = 0.0F;

6794


6795
    /* InitializeConditions for Memory: '<S433>/Memory3' */

6796
    ((real32_T *)ssGetDWork(S, 279))[0] = 0.0F;

6797


6798
    /* InitializeConditions for Memory: '<S433>/Memory4' */

6799
    ((real32_T *)ssGetDWork(S, 280))[0] = 0.0F;

6800


6801
    /* InitializeConditions for Memory: '<S433>/Memory5' */

6802
    ((real32_T *)ssGetDWork(S, 281))[0] = 0.0F;

6803


6804
    /* InitializeConditions for Memory: '<S433>/Memory6' */

6805
    ((real32_T *)ssGetDWork(S, 282))[0] = 0.0F;

6806


6807
    /* InitializeConditions for Memory: '<S433>/Memory7' */

6808
    ((real32_T *)ssGetDWork(S, 283))[0] = 0.0F;

6809


6810
    /* InitializeConditions for Memory: '<S433>/Memory8' */

6811
    ((real32_T *)ssGetDWork(S, 284))[0] = 0.0F;

6812


6813
    /* InitializeConditions for Memory: '<S433>/Memory9' */

6814
    ((real32_T *)ssGetDWork(S, 285))[0] = 0.0F;

6815


6816
    /* SystemInitialize for Atomic SubSystem: '<S1>/Subsystem5' */

6817
    /* SystemInitialize for Enabled SubSystem: '<S15>/Subsystem3' */

6818
    /* InitializeConditions for Integrator: '<S472>/Servo rate' */

6819
    _rtX->Servorate_CSTATE = 0.0;

6820


6821
    /* InitializeConditions for Integrator: '<S472>/Limited Deflection' */

6822
    _rtX->LimitedDeflection_CSTATE = 0.0;

6823


6824
    /* InitializeConditions for Integrator: '<S473>/Servo rate' */

6825
    _rtX->Servorate_CSTATE_n = 0.0;

6826


6827
    /* InitializeConditions for Integrator: '<S473>/Limited Deflection' */

6828
    _rtX->LimitedDeflection_CSTATE_i = 0.0;

6829


6830
    /* InitializeConditions for Integrator: '<S474>/Servo rate' */

6831
    _rtX->Servorate_CSTATE_m = 0.0;

6832


6833
    /* InitializeConditions for Integrator: '<S474>/Limited Deflection' */

6834
    _rtX->LimitedDeflection_CSTATE_m = 0.0;

6835


6836
    /* InitializeConditions for Integrator: '<S475>/Servo rate' */

6837
    _rtX->Servorate_CSTATE_mi = 0.0;

6838


6839
    /* InitializeConditions for Integrator: '<S475>/Limited Deflection' */

6840
    _rtX->LimitedDeflection_CSTATE_f = 0.0;

6841


6842
    /* InitializeConditions for Integrator: '<S476>/Servo rate' */

6843
    _rtX->Servorate_CSTATE_f = 0.0;

6844


6845
    /* InitializeConditions for Integrator: '<S476>/Limited Deflection' */

6846
    _rtX->LimitedDeflection_CSTATE_g = 0.0;

6847


6848
    /* InitializeConditions for Integrator: '<S477>/Servo rate' */

6849
    _rtX->Servorate_CSTATE_k = 0.0;

6850


6851
    /* InitializeConditions for Integrator: '<S477>/Limited Deflection' */

6852
    _rtX->LimitedDeflection_CSTATE_l = 0.0;

6853


6854
    /* InitializeConditions for Integrator: '<S478>/Servo rate' */

6855
    _rtX->Servorate_CSTATE_h = 0.0;

6856


6857
    /* InitializeConditions for Integrator: '<S478>/Limited Deflection' */

6858
    _rtX->LimitedDeflection_CSTATE_j = 0.0;

6859


6860
    /* InitializeConditions for Integrator: '<S479>/Servo rate' */

6861
    _rtX->Servorate_CSTATE_kx = 0.0;

6862


6863
    /* InitializeConditions for Integrator: '<S479>/Limited Deflection' */

6864
    _rtX->LimitedDeflection_CSTATE_la = 0.0;

6865


6866
    /* InitializeConditions for TransferFcn: '<S462>/TransferFcn1' */

6867
    _rtX->TransferFcn1_CSTATE = 0.0;

6868


6869
    /* InitializeConditions for Integrator: '<S779>/q' incorporates:

6870
     *  InitializeConditions for Integrator: '<S778>/p1'

6871
     */

6872
    if (ssIsFirstInitCond(S)) {

6873
      _rtX->q_CSTATE[0] = 0.0;

6874
      _rtX->q_CSTATE[1] = 0.0;

6875
      _rtX->q_CSTATE[2] = 0.0;

6876
      _rtX->q_CSTATE[3] = 0.0;

6877
      _rtX->p1_CSTATE[0] = 0.0;

6878
      _rtX->p1_CSTATE[1] = 0.0;

6879
      _rtX->p1_CSTATE[2] = 0.0;

6880
    }

6881


6882
    ((int_T *)ssGetDWork(S, 344))[0] = 1;

6883


6884
    /* End of InitializeConditions for Integrator: '<S779>/q' */

6885


6886
    /* InitializeConditions for Integrator: '<S778>/p1' */

6887
    ((int_T *)ssGetDWork(S, 345))[0] = 1;

6888


6889
    /* InitializeConditions for RateLimiter: '<S465>/Rate Limiter' */

6890
    ((real_T *)ssGetDWork(S, 3))[0] = (rtInf);

6891


6892
    /* InitializeConditions for Integrator: '<S780>/ub,vb,wb' incorporates:

6893
     *  InitializeConditions for Integrator: '<S470>/p,q,r '

6894
     */

6895
    if (ssIsFirstInitCond(S)) {

6896
      _rtX->ubvbwb_CSTATE[0] = 0.0;

6897
      _rtX->ubvbwb_CSTATE[1] = 0.0;

6898
      _rtX->ubvbwb_CSTATE[2] = 0.0;

6899
      _rtX->pqr_CSTATE[0] = 0.0;

6900
      _rtX->pqr_CSTATE[1] = 0.0;

6901
      _rtX->pqr_CSTATE[2] = 0.0;

6902
    }

6903


6904
    ((int_T *)ssGetDWork(S, 346))[0] = 1;

6905


6906
    /* End of InitializeConditions for Integrator: '<S780>/ub,vb,wb' */

6907


6908
    /* InitializeConditions for Backlash: '<S446>/Backlash' */

6909
    ((real_T *)ssGetDWork(S, 4))[0] = 0.0;

6910


6911
    /* InitializeConditions for Integrator: '<S470>/p,q,r ' */

6912
    ((int_T *)ssGetDWork(S, 347))[0] = 1;

6913


6914
    /* InitializeConditions for UnitDelay: '<S682>/Unit Delay' */

6915
    ((int8_T *)ssGetDWork(S, 371))[0] = 0;

6916


6917
    /* InitializeConditions for UnitDelay: '<S683>/Unit Delay' */

6918
    ((int8_T *)ssGetDWork(S, 372))[0] = 0;

6919


6920
    /* InitializeConditions for UnitDelay: '<S619>/Unit Delay1' */

6921
    ((real_T *)ssGetDWork(S, 0))[0] = 0.0;

6922


6923
    /* InitializeConditions for Integrator: '<S683>/Integrator' incorporates:

6924
     *  InitializeConditions for Integrator: '<S682>/Integrator'

6925
     */

6926
    if (ssIsFirstInitCond(S)) {

6927
      _rtX->Integrator_CSTATE = 0.0;

6928
      _rtX->Integrator_CSTATE_l = 0.0;

6929
    }

6930


6931
    ((int_T *)ssGetDWork(S, 348))[0] = 1;

6932


6933
    /* End of InitializeConditions for Integrator: '<S683>/Integrator' */

6934


6935
    /* InitializeConditions for Integrator: '<S682>/Integrator' */

6936
    ((int_T *)ssGetDWork(S, 349))[0] = 1;

6937


6938
    /* InitializeConditions for TransferFcn: '<S683>/Transfer Fcn2' */

6939
    _rtX->TransferFcn2_CSTATE[0] = 0.0;

6940
    _rtX->TransferFcn2_CSTATE[1] = 0.0;

6941


6942
    /* InitializeConditions for Integrator: '<S781>/Integrator' */

6943
    _rtX->Integrator_CSTATE_a = 0.0;

6944


6945
    /* InitializeConditions for Integrator: '<S778>/p' */

6946
    if (ssIsFirstInitCond(S)) {

6947
      _rtX->p_CSTATE[0] = 0.0;

6948
      _rtX->p_CSTATE[1] = 0.0;

6949
      _rtX->p_CSTATE[2] = 0.0;

6950
    }

6951


6952
    ((int_T *)ssGetDWork(S, 350))[0] = 1;

6953


6954
    /* End of InitializeConditions for Integrator: '<S778>/p' */

6955


6956
    /* InitializeConditions for TransferFcn: '<S446>/Transfer Fcn' */

6957
    _rtX->TransferFcn_CSTATE = 0.0;

6958


6959
    /* SystemInitialize for Merge: '<S847>/Merge' */

6960
    _rtB->Merge[0] = 1.0;

6961
    _rtB->Merge[1] = 0.0;

6962
    _rtB->Merge[2] = 0.0;

6963
    _rtB->Merge[3] = 0.0;

6964


6965
    /* SystemInitialize for Iterator SubSystem: '<S774>/While Iterator Subsystem' */

6966
    /* InitializeConditions for Memory: '<S793>/Memory' */

6967
    ((real_T *)ssGetDWork(S, 11))[0] = 0.0;

6968


6969
    /* SystemInitialize for Outport: '<S787>/phi' */

6970
    _rtB->TrigonometricFunction4 = 0.0;

6971


6972
    /* End of SystemInitialize for SubSystem: '<S774>/While Iterator Subsystem' */

6973


6974
    /* SystemInitialize for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

6975
    AFCS_MODEL1_LMainGearGeometryForcesandMoments_Init(S,

6976
      &_rtB->LMainGearGeometryForcesandMomen);

6977


6978
    /* End of SystemInitialize for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

6979


6980
    /* SystemInitialize for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

6981
    AFCS_MODEL1_RMainGearGeometryForcesandMoments_Init(S,

6982
      &_rtB->RMainGearGeometryForcesandMomen);

6983


6984
    /* End of SystemInitialize for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

6985


6986
    /* SystemInitialize for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

6987
    AFCS_MODEL1_NoseGearGeometryForcesandMoments_Init(S,

6988
      &_rtB->NoseGearGeometryForcesandMoment);

6989


6990
    /* End of SystemInitialize for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

6991


6992
    /* SystemInitialize for Outport: '<S446>/Sensor1' */

6993
    _rtB->phiL = 0.0;

6994
    _rtB->thetaL = 0.0;

6995
    _rtB->psiL = 0.0;

6996
    _rtB->p = 0.0;

6997
    _rtB->q = 0.0;

6998
    _rtB->r = 0.0;

6999
    _rtB->Abx = 0.0;

7000
    _rtB->Aby = 0.0;

7001
    _rtB->Abz = 0.0;

7002
    _rtB->Lat_j = 0.0;

7003
    _rtB->Lon_e = 0.0;

7004
    _rtB->Altitude_j = 0.0;

7005
    _rtB->GPS_vnorth_i = 0.0;

7006
    _rtB->GPS_veast_k = 0.0;

7007
    _rtB->GPS_vdown_k = 0.0;

7008
    _rtB->Alfa = 0.0;

7009
    _rtB->Beta = 0.0;

7010
    _rtB->Qbar = 0.0;

7011
    _rtB->Va = 0.0;

7012


7013
    /* SystemInitialize for Outport: '<S446>/ENT' */

7014
    _rtB->OutportBufferForENT[0] = 0.0;

7015
    _rtB->OutportBufferForENT[1] = 0.0;

7016
    _rtB->OutportBufferForENT[2] = 0.0;

7017


7018
    /* End of SystemInitialize for SubSystem: '<S15>/Subsystem3' */

7019
    /* End of SystemInitialize for SubSystem: '<S1>/Subsystem5' */

7020


7021
    /* SystemInitialize for Enabled SubSystem: '<S299>/Enabled Subsystem' */

7022
    /* SystemInitialize for Outport: '<S302>/A' */

7023
    memset(&_rtB->OutportBufferForA[0], 0, 120U * sizeof(real_T));

7024


7025
    /* SystemInitialize for Outport: '<S302>/NumPoint' */

7026
    _rtB->airNumPoint = 0;

7027


7028
    /* End of SystemInitialize for SubSystem: '<S299>/Enabled Subsystem' */

7029


7030
    /* SystemInitialize for MATLAB Function: '<S293>/CompTP' */

7031
    ((int8_T *)ssGetDWork(S, 516))[0] = 0;

7032
    ((real32_T *)ssGetDWork(S, 287))[0] = 0.0F;

7033
    ((real32_T *)ssGetDWork(S, 288))[0] = 0.0F;

7034


7035
    /* SystemInitialize for Chart: '<S24>/zeros2' */

7036
    ((uint8_T *)ssGetDWork(S, 586))[0] = 0U;

7037
    ((uint8_T *)ssGetDWork(S, 587))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7038


7039
    /* SystemInitialize for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */

7040
    /* InitializeConditions for UnitDelay: '<S423>/Output' */

7041
    ((uint64_T *)ssGetDWork(S, 1))[0] = 0ULL;

7042


7043
    /* End of SystemInitialize for SubSystem: '<S9>/Trajectory planning,tracking' */

7044


7045
    /* SystemInitialize for Chart: '<S21>/delayblock3' */

7046
    ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7047
    ((uint8_T *)ssGetDWork(S, 603))[0] = 0U;

7048
    ((uint8_T *)ssGetDWork(S, 604))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7049


7050
    /* SystemInitialize for MATLAB Function: '<S31>/LogicTrans' */

7051
    ((int8_T *)ssGetDWork(S, 514))[0] = 0;

7052


7053
    /* SystemInitialize for Chart: '<S21>/delayblock2' */

7054
    ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7055
    ((uint8_T *)ssGetDWork(S, 606))[0] = 0U;

7056
    ((uint8_T *)ssGetDWork(S, 607))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7057


7058
    /* SystemInitialize for Chart: '<S32>/wowcmp1' */

7059
    ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7060
    ((uint8_T *)ssGetDWork(S, 524))[0] = 0U;

7061
    ((uint8_T *)ssGetDWork(S, 525))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7062


7063
    /* SystemInitialize for Chart: '<S21>/delayblock5' */

7064
    ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7065
    ((uint8_T *)ssGetDWork(S, 597))[0] = 0U;

7066
    ((uint8_T *)ssGetDWork(S, 598))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7067


7068
    /* SystemInitialize for Chart: '<S21>/delayblock7' */

7069
    ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7070
    ((uint8_T *)ssGetDWork(S, 591))[0] = 0U;

7071
    ((uint8_T *)ssGetDWork(S, 592))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7072


7073
    /* SystemInitialize for Chart: '<S21>/delayblock4' */

7074
    ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7075
    ((uint8_T *)ssGetDWork(S, 600))[0] = 0U;

7076
    ((uint8_T *)ssGetDWork(S, 601))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7077


7078
    /* SystemInitialize for IfAction SubSystem: '<S31>/Pre-Takeoff' */

7079
    /* SystemInitialize for Chart: '<S319>/Pre_Takeoff' */

7080
    ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7081
    ((uint8_T *)ssGetDWork(S, 580))[0] = 0U;

7082
    ((uint8_T *)ssGetDWork(S, 581))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7083


7084
    /* End of SystemInitialize for SubSystem: '<S31>/Pre-Takeoff' */

7085


7086
    /* SystemInitialize for IfAction SubSystem: '<S31>/Takeoff' */

7087
    /* SystemInitialize for Chart: '<S321>/Takeoff' */

7088
    ((uint8_T *)ssGetDWork(S, 578))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7089
    ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7090
    ((uint8_T *)ssGetDWork(S, 576))[0] = 0U;

7091
    ((uint8_T *)ssGetDWork(S, 577))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7092


7093
    /* End of SystemInitialize for SubSystem: '<S31>/Takeoff' */

7094


7095
    /* SystemInitialize for IfAction SubSystem: '<S31>/Climb' */

7096
    /* SystemInitialize for Chart: '<S314>/Climb2' */

7097
    ((uint8_T *)ssGetDWork(S, 569))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7098
    ((uint8_T *)ssGetDWork(S, 570))[0] = 0U;

7099
    ((uint8_T *)ssGetDWork(S, 575))[0] = 0U;

7100
    ((uint8_T *)ssGetDWork(S, 572))[0] = 0U;

7101
    ((uint8_T *)ssGetDWork(S, 571))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7102
    ((uint8_T *)ssGetDWork(S, 573))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7103
    ((uint8_T *)ssGetDWork(S, 574))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7104
    ((uint8_T *)ssGetDWork(S, 567))[0] = 0U;

7105
    ((uint8_T *)ssGetDWork(S, 568))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7106


7107
    /* End of SystemInitialize for SubSystem: '<S31>/Climb' */

7108


7109
    /* SystemInitialize for IfAction SubSystem: '<S31>/Cruise' */

7110
    /* SystemInitialize for Chart: '<S315>/Cruise' */

7111
    ((uint8_T *)ssGetDWork(S, 560))[0] = 0U;

7112
    ((uint8_T *)ssGetDWork(S, 559))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7113
    ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7114
    ((uint8_T *)ssGetDWork(S, 563))[0] = 0U;

7115
    ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7116
    ((uint8_T *)ssGetDWork(S, 565))[0] = 0U;

7117
    ((uint8_T *)ssGetDWork(S, 564))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7118
    ((uint8_T *)ssGetDWork(S, 566))[0] = 0U;

7119
    ((uint8_T *)ssGetDWork(S, 557))[0] = 0U;

7120
    ((uint8_T *)ssGetDWork(S, 558))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7121


7122
    /* End of SystemInitialize for SubSystem: '<S31>/Cruise' */

7123


7124
    /* SystemInitialize for IfAction SubSystem: '<S31>/Approach' */

7125
    /* SystemInitialize for Chart: '<S313>/Approach2' */

7126
    ((uint8_T *)ssGetDWork(S, 549))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7127
    ((uint8_T *)ssGetDWork(S, 550))[0] = 0U;

7128
    ((uint8_T *)ssGetDWork(S, 552))[0] = 0U;

7129
    ((uint8_T *)ssGetDWork(S, 551))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7130
    ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7131
    ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7132
    ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7133
    ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7134
    ((uint8_T *)ssGetDWork(S, 547))[0] = 0U;

7135
    ((uint8_T *)ssGetDWork(S, 548))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7136


7137
    /* End of SystemInitialize for SubSystem: '<S31>/Approach' */

7138


7139
    /* SystemInitialize for IfAction SubSystem: '<S31>/Landing' */

7140
    /* SystemInitialize for Chart: '<S316>/Landing2' */

7141
    ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7142
    ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7143
    ((uint8_T *)ssGetDWork(S, 540))[0] = 0U;

7144
    ((uint8_T *)ssGetDWork(S, 542))[0] = 0U;

7145
    ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7146
    ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7147
    ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7148
    ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7149
    ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7150
    ((uint8_T *)ssGetDWork(S, 535))[0] = 0U;

7151
    ((uint8_T *)ssGetDWork(S, 537))[0] = 0U;

7152
    ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7153
    ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7154
    ((uint8_T *)ssGetDWork(S, 532))[0] = 0U;

7155
    ((uint8_T *)ssGetDWork(S, 533))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7156


7157
    /* End of SystemInitialize for SubSystem: '<S31>/Landing' */

7158


7159
    /* SystemInitialize for IfAction SubSystem: '<S31>/Taxi' */

7160
    /* SystemInitialize for Chart: '<S322>/Taxi' */

7161
    ((uint8_T *)ssGetDWork(S, 531))[0] = 0U;

7162
    ((uint8_T *)ssGetDWork(S, 530))[0] = 0U;

7163
    ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7164
    ((uint8_T *)ssGetDWork(S, 527))[0] = 0U;

7165
    ((uint8_T *)ssGetDWork(S, 528))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7166


7167
    /* End of SystemInitialize for SubSystem: '<S31>/Taxi' */

7168


7169
    /* SystemInitialize for Merge: '<S31>/Merge' */

7170
    _rtB->vtcmd = 0.0F;

7171


7172
    /* SystemInitialize for Merge: '<S31>/Merge' */

7173
    _rtB->nycmd = 0.0F;

7174


7175
    /* SystemInitialize for Merge: '<S31>/Merge' */

7176
    _rtB->phiLcmd = 0.0F;

7177


7178
    /* SystemInitialize for Merge: '<S31>/Merge' */

7179
    _rtB->thetaLcmd = 0.0F;

7180


7181
    /* SystemInitialize for Merge: '<S31>/Merge' */

7182
    _rtB->psiLcmd = 0.0F;

7183


7184
    /* SystemInitialize for Merge: '<S31>/Merge' */

7185
    _rtB->gamacmd = 0.0F;

7186


7187
    /* SystemInitialize for Merge: '<S31>/Merge' */

7188
    _rtB->hcmd = 0.0F;

7189


7190
    /* SystemInitialize for Merge: '<S31>/Merge' */

7191
    _rtB->psdcmd = 0.0F;

7192


7193
    /* SystemInitialize for Merge: '<S31>/Merge' */

7194
    _rtB->yrwcmd = 0.0F;

7195


7196
    /* SystemInitialize for Merge: '<S31>/Merge' */

7197
    _rtB->apthL = 0;

7198


7199
    /* SystemInitialize for Merge: '<S31>/Merge' */

7200
    _rtB->apfix = 0;

7201


7202
    /* SystemInitialize for Merge: '<S31>/Merge' */

7203
    _rtB->aplonL = 0;

7204


7205
    /* SystemInitialize for Merge: '<S31>/Merge' */

7206
    _rtB->aplatL = 0;

7207


7208
    /* SystemInitialize for Merge: '<S31>/Merge' */

7209
    _rtB->apyawL = 0;

7210


7211
    /* SystemInitialize for Merge: '<S31>/Merge' */

7212
    _rtB->apsbL = 0;

7213


7214
    /* SystemInitialize for Merge: '<S31>/Merge' */

7215
    _rtB->apnwsL = 0;

7216


7217
    /* SystemInitialize for Merge: '<S31>/Merge' */

7218
    _rtB->apbrkL = 0;

7219


7220
    /* SystemInitialize for Merge: '<S31>/Merge' */

7221
    _rtB->thrustcmd = 0.0F;

7222


7223
    /* SystemInitialize for Atomic SubSystem: '<S7>/Level' */

7224


7225
    /* SystemInitialize for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */

7226
    AFCS_MODEL1_CodeReuseSubsystem_Init(S);

7227


7228
    /* End of SystemInitialize for SubSystem: '<S22>/CodeReuseSubsystem' */

7229


7230
    /* End of SystemInitialize for SubSystem: '<S7>/Level' */

7231


7232
    /* SystemInitialize for Merge: '<S31>/Merge' */

7233
    _rtB->configure = 0;

7234


7235
    /* SystemInitialize for Merge: '<S31>/Merge' */

7236
    _rtB->nzcmd = 0.0F;

7237


7238
    /* SystemInitialize for Merge: '<S31>/Merge' */

7239
    _rtB->K = 0;

7240


7241
    /* SystemInitialize for Chart: '<S21>/delayblock1' */

7242
    ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7243
    ((uint8_T *)ssGetDWork(S, 609))[0] = 0U;

7244
    ((uint8_T *)ssGetDWork(S, 610))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7245


7246
    /* SystemInitialize for Chart: '<S21>/delayblock6' */

7247
    ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7248
    ((uint8_T *)ssGetDWork(S, 594))[0] = 0U;

7249
    ((uint8_T *)ssGetDWork(S, 595))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7250


7251
    /* SystemInitialize for Chart: '<S299>/AirLineUpdateSucModel' */

7252
    ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7253
    ((uint8_T *)ssGetDWork(S, 588))[0] = 0U;

7254
    ((uint8_T *)ssGetDWork(S, 589))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7255


7256
    /* SystemInitialize for Chart: '<S29>/LLACalibration2' */

7257
    ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7258
    ((uint8_T *)ssGetDWork(S, 583))[0] = 0U;

7259
    ((uint8_T *)ssGetDWork(S, 584))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7260


7261
    /* SystemInitialize for Merge: '<S31>/Merge' */

7262
    _rtB->apcheck = 0;

7263


7264
    /* SystemInitialize for Merge: '<S31>/Merge' */

7265
    _rtB->StartEmNav = 0;

7266


7267
    /* SystemInitialize for Chart: '<S12>/LLACalibration2' */

7268
    ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7269
    ((uint8_T *)ssGetDWork(S, 521))[0] = 0U;

7270
    ((uint8_T *)ssGetDWork(S, 522))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7271
  } else {

7272
    /* InitializeConditions for Memory: '<S433>/Memory10' */

7273
    ((real32_T *)ssGetDWork(S, 270))[0] = 0.0F;

7274


7275
    /* InitializeConditions for Memory: '<S438>/Memory1' */

7276
    ((real32_T *)ssGetDWork(S, 271))[0] = 0.0F;

7277


7278
    /* InitializeConditions for Memory: '<S437>/Memory1' */

7279
    ((real32_T *)ssGetDWork(S, 272))[0] = 0.0F;

7280


7281
    /* InitializeConditions for Memory: '<S436>/Memory1' */

7282
    ((real32_T *)ssGetDWork(S, 273))[0] = 0.0F;

7283


7284
    /* InitializeConditions for Memory: '<S435>/Memory1' */

7285
    ((real32_T *)ssGetDWork(S, 274))[0] = 0.0F;

7286


7287
    /* InitializeConditions for Memory: '<S434>/Memory1' */

7288
    ((real32_T *)ssGetDWork(S, 275))[0] = 0.0F;

7289


7290
    /* InitializeConditions for Memory: '<S439>/Memory1' */

7291
    ((real32_T *)ssGetDWork(S, 276))[0] = 0.0F;

7292


7293
    /* InitializeConditions for UnitDelay: '<S24>/Unit Delay3' */

7294
    ((int8_T *)ssGetDWork(S, 352))[0] = 0;

7295


7296
    /* InitializeConditions for UnitDelay: '<S7>/Unit Delay' */

7297
    ((int8_T *)ssGetDWork(S, 353))[0] = 1;

7298


7299
    /* InitializeConditions for UnitDelay: '<S7>/Unit Delay3' */

7300
    ((int8_T *)ssGetDWork(S, 354))[0] = 0;

7301


7302
    /* InitializeConditions for Memory: '<S9>/Memory1' */

7303
    ((boolean_T *)ssGetDWork(S, 612))[0] = false;

7304


7305
    /* InitializeConditions for Memory: '<S9>/Memory2' */

7306
    ((int8_T *)ssGetDWork(S, 504))[0] = 0;

7307


7308
    /* InitializeConditions for UnitDelay: '<S9>/Unit Delay' */

7309
    ((int8_T *)ssGetDWork(S, 355))[0] = 0;

7310


7311
    /* InitializeConditions for UnitDelay: '<S1>/Unit Delay1' */

7312
    ((int8_T *)ssGetDWork(S, 356))[0] = 0;

7313


7314
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay' */

7315
    ((int8_T *)ssGetDWork(S, 357))[0] = 0;

7316


7317
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay1' */

7318
    ((int8_T *)ssGetDWork(S, 358))[0] = 0;

7319


7320
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay2' */

7321
    ((int8_T *)ssGetDWork(S, 359))[0] = 0;

7322


7323
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay3' */

7324
    ((int8_T *)ssGetDWork(S, 360))[0] = 0;

7325


7326
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay4' */

7327
    ((int8_T *)ssGetDWork(S, 361))[0] = 0;

7328


7329
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay7' */

7330
    ((int8_T *)ssGetDWork(S, 362))[0] = 0;

7331


7332
    /* InitializeConditions for UnitDelay: '<S31>/Unit Delay5' */

7333
    ((int8_T *)ssGetDWork(S, 363))[0] = 0;

7334


7335
    /* InitializeConditions for UnitDelay: '<S32>/Unit Delay3' */

7336
    ((int8_T *)ssGetDWork(S, 364))[0] = 0;

7337


7338
    /* InitializeConditions for Delay: '<S310>/Delay' */

7339
    ((boolean_T *)ssGetDWork(S, 502))[0] = false;

7340


7341
    /* InitializeConditions for UnitDelay: '<S309>/Unit Delay6' */

7342
    ((int8_T *)ssGetDWork(S, 365))[0] = 0;

7343


7344
    /* InitializeConditions for UnitDelay: '<S309>/Unit Delay5' */

7345
    ((int8_T *)ssGetDWork(S, 366))[0] = 0;

7346


7347
    /* InitializeConditions for UnitDelay: '<S1>/Unit Delay' */

7348
    ((int8_T *)ssGetDWork(S, 367))[0] = 0;

7349


7350
    /* InitializeConditions for Delay: '<S431>/Delay' */

7351
    ((boolean_T *)ssGetDWork(S, 503))[0] = false;

7352


7353
    /* InitializeConditions for UnitDelay: '<S430>/Unit Delay6' */

7354
    ((int8_T *)ssGetDWork(S, 368))[0] = 0;

7355


7356
    /* InitializeConditions for UnitDelay: '<S336>/Unit Delay2' */

7357
    ((int8_T *)ssGetDWork(S, 369))[0] = 0;

7358


7359
    /* InitializeConditions for UnitDelay: '<S430>/Unit Delay5' */

7360
    ((int8_T *)ssGetDWork(S, 370))[0] = 0;

7361


7362
    /* InitializeConditions for Memory: '<S433>/Memory1' */

7363
    ((real32_T *)ssGetDWork(S, 277))[0] = 0.0F;

7364


7365
    /* InitializeConditions for Memory: '<S433>/Memory2' */

7366
    ((real32_T *)ssGetDWork(S, 278))[0] = 0.0F;

7367


7368
    /* InitializeConditions for Memory: '<S433>/Memory3' */

7369
    ((real32_T *)ssGetDWork(S, 279))[0] = 0.0F;

7370


7371
    /* InitializeConditions for Memory: '<S433>/Memory4' */

7372
    ((real32_T *)ssGetDWork(S, 280))[0] = 0.0F;

7373


7374
    /* InitializeConditions for Memory: '<S433>/Memory5' */

7375
    ((real32_T *)ssGetDWork(S, 281))[0] = 0.0F;

7376


7377
    /* InitializeConditions for Memory: '<S433>/Memory6' */

7378
    ((real32_T *)ssGetDWork(S, 282))[0] = 0.0F;

7379


7380
    /* InitializeConditions for Memory: '<S433>/Memory7' */

7381
    ((real32_T *)ssGetDWork(S, 283))[0] = 0.0F;

7382


7383
    /* InitializeConditions for Memory: '<S433>/Memory8' */

7384
    ((real32_T *)ssGetDWork(S, 284))[0] = 0.0F;

7385


7386
    /* InitializeConditions for Memory: '<S433>/Memory9' */

7387
    ((real32_T *)ssGetDWork(S, 285))[0] = 0.0F;

7388


7389
    /* SystemReset for MATLAB Function: '<S293>/CompTP' */

7390
    ((int8_T *)ssGetDWork(S, 516))[0] = 0;

7391
    ((real32_T *)ssGetDWork(S, 287))[0] = 0.0F;

7392
    ((real32_T *)ssGetDWork(S, 288))[0] = 0.0F;

7393


7394
    /* SystemReset for Chart: '<S24>/zeros2' */

7395
    ((uint8_T *)ssGetDWork(S, 586))[0] = 0U;

7396
    ((uint8_T *)ssGetDWork(S, 587))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7397


7398
    /* SystemReset for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */

7399
    /* InitializeConditions for UnitDelay: '<S423>/Output' */

7400
    ((uint64_T *)ssGetDWork(S, 1))[0] = 0ULL;

7401


7402
    /* End of SystemReset for SubSystem: '<S9>/Trajectory planning,tracking' */

7403


7404
    /* SystemReset for Chart: '<S21>/delayblock3' */

7405
    ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7406
    ((uint8_T *)ssGetDWork(S, 603))[0] = 0U;

7407
    ((uint8_T *)ssGetDWork(S, 604))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7408


7409
    /* SystemReset for MATLAB Function: '<S31>/LogicTrans' */

7410
    ((int8_T *)ssGetDWork(S, 514))[0] = 0;

7411


7412
    /* SystemReset for Chart: '<S21>/delayblock2' */

7413
    ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7414
    ((uint8_T *)ssGetDWork(S, 606))[0] = 0U;

7415
    ((uint8_T *)ssGetDWork(S, 607))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7416


7417
    /* SystemReset for Chart: '<S32>/wowcmp1' */

7418
    ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7419
    ((uint8_T *)ssGetDWork(S, 524))[0] = 0U;

7420
    ((uint8_T *)ssGetDWork(S, 525))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7421


7422
    /* SystemReset for Chart: '<S21>/delayblock5' */

7423
    ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7424
    ((uint8_T *)ssGetDWork(S, 597))[0] = 0U;

7425
    ((uint8_T *)ssGetDWork(S, 598))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7426


7427
    /* SystemReset for Chart: '<S21>/delayblock7' */

7428
    ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7429
    ((uint8_T *)ssGetDWork(S, 591))[0] = 0U;

7430
    ((uint8_T *)ssGetDWork(S, 592))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7431


7432
    /* SystemReset for Chart: '<S21>/delayblock4' */

7433
    ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7434
    ((uint8_T *)ssGetDWork(S, 600))[0] = 0U;

7435
    ((uint8_T *)ssGetDWork(S, 601))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7436


7437
    /* SystemReset for Atomic SubSystem: '<S7>/Level' */

7438


7439
    /* SystemReset for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */

7440
    AFCS_MODEL1_CodeReuseSubsystem_Reset(S);

7441


7442
    /* End of SystemReset for SubSystem: '<S22>/CodeReuseSubsystem' */

7443


7444
    /* End of SystemReset for SubSystem: '<S7>/Level' */

7445


7446
    /* SystemReset for Chart: '<S21>/delayblock1' */

7447
    ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7448
    ((uint8_T *)ssGetDWork(S, 609))[0] = 0U;

7449
    ((uint8_T *)ssGetDWork(S, 610))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7450


7451
    /* SystemReset for Chart: '<S21>/delayblock6' */

7452
    ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7453
    ((uint8_T *)ssGetDWork(S, 594))[0] = 0U;

7454
    ((uint8_T *)ssGetDWork(S, 595))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7455


7456
    /* SystemReset for Chart: '<S299>/AirLineUpdateSucModel' */

7457
    ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7458
    ((uint8_T *)ssGetDWork(S, 588))[0] = 0U;

7459
    ((uint8_T *)ssGetDWork(S, 589))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7460


7461
    /* SystemReset for Chart: '<S29>/LLACalibration2' */

7462
    ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7463
    ((uint8_T *)ssGetDWork(S, 583))[0] = 0U;

7464
    ((uint8_T *)ssGetDWork(S, 584))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7465


7466
    /* SystemReset for Chart: '<S12>/LLACalibration2' */

7467
    ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7468
    ((uint8_T *)ssGetDWork(S, 521))[0] = 0U;

7469
    ((uint8_T *)ssGetDWork(S, 522))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

7470
  }

7471
}

7472


7473
/* Disable for root system: '<Root>' */

7474
#define RTW_GENERATED_DISABLE

7475


7476
static void mdlDisable(SimStruct *S)

7477
{

7478
  B_AFCS_MODEL1_T *_rtB;

7479
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

7480


7481
  /* Disable for Atomic SubSystem: '<S1>/Subsystem5' */

7482
  /* Disable for Enabled SubSystem: '<S15>/Subsystem3' */

7483
  if (((boolean_T *)ssGetDWork(S, 614))[0]) {

7484
    /* Disable for If: '<S847>/If' */

7485
    ((int8_T *)ssGetDWork(S, 506))[0] = -1;

7486


7487
    /* Disable for If: '<S846>/If' */

7488
    ((int8_T *)ssGetDWork(S, 507))[0] = -1;

7489


7490
    /* Disable for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

7491
    AFCS_MODEL1_LMainGearGeometryForcesandMoments_Disable(S,

7492
      &_rtB->LMainGearGeometryForcesandMomen);

7493


7494
    /* End of Disable for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

7495


7496
    /* Disable for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

7497
    AFCS_MODEL1_RMainGearGeometryForcesandMoments_Disable(S,

7498
      &_rtB->RMainGearGeometryForcesandMomen);

7499


7500
    /* End of Disable for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

7501


7502
    /* Disable for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

7503
    AFCS_MODEL1_NoseGearGeometryForcesandMoments_Disable(S,

7504
      &_rtB->NoseGearGeometryForcesandMoment);

7505


7506
    /* End of Disable for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

7507


7508
    /* Disable for If: '<S848>/If' */

7509
    ((int8_T *)ssGetDWork(S, 508))[0] = -1;

7510
    ((boolean_T *)ssGetDWork(S, 614))[0] = false;

7511
  }

7512


7513
  /* End of Disable for SubSystem: '<S15>/Subsystem3' */

7514
  /* End of Disable for SubSystem: '<S1>/Subsystem5' */

7515


7516
  /* Disable for Enabled SubSystem: '<S299>/Enabled Subsystem' */

7517
  if (((boolean_T *)ssGetDWork(S, 613))[0]) {

7518
    ((boolean_T *)ssGetDWork(S, 613))[0] = false;

7519
  }

7520


7521
  /* End of Disable for SubSystem: '<S299>/Enabled Subsystem' */

7522


7523
  /* Disable for SwitchCase: '<S31>/Switch Case' */

7524
  if (((int8_T *)ssGetDWork(S, 505))[0] == 3) {

7525
    ssSetSolverNeedsReset(S);

7526
  }

7527


7528
  ((int8_T *)ssGetDWork(S, 505))[0] = -1;

7529


7530
  /* End of Disable for SwitchCase: '<S31>/Switch Case' */

7531
}

7532


7533
/* Start for root system: '<Root>' */

7534
#define MDL_START

7535


7536
static void mdlStart(SimStruct *S)

7537
{

7538
  /* instance underlying S-Function data */

7539
#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)

7540
#  if defined(MATLAB_MEX_FILE)

7541


7542
  /* non-finites */

7543
  rt_InitInfAndNaN(sizeof(real_T));

7544


7545
  /* Check for invalid switching between solver types */

7546
  if (ssIsVariableStepSolver(S)) {

7547
    ssSetErrorStatus(S, "This Simulink Coder generated "

7548
                     "S-Function cannot be used in a simulation with "

7549
                     "a solver type of variable-step "

7550
                     "because this S-Function was created from a model with "

7551
                     "solver type of fixed-step and it has continuous time blocks. "

7552
                     "See the Solver page of the simulation parameters dialog.");

7553
    return;

7554
  }

7555


7556
  if (ssGetSolverMode(S) == SOLVER_MODE_MULTITASKING) {

7557
    ssSetErrorStatus(S, "This Simulink Coder generated "

7558
                     "S-Function cannot be used in a simulation with "

7559
                     "solver mode set to auto or multitasking "

7560
                     "because this S-Function was created from a model with "

7561
                     "solver mode set to singletasking. "

7562
                     "See the Solver page of the simulation parameters dialog.");

7563
    return;

7564
  }

7565


7566
#  endif

7567


7568
  AFCS_MODEL1_malloc(S);

7569
  if (ssGetErrorStatus(S) != (NULL) ) {

7570
    return;

7571
  }

7572


7573
#endif

7574


7575
  {

7576
    B_AFCS_MODEL1_T *_rtB;

7577
    _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

7578


7579
    /* Start for Atomic SubSystem: '<S1>/Subsystem5' */

7580
    /* Start for Constant: '<S440>/Constant3' */

7581
    _rtB->uvw0[0] = 0.0;

7582
    _rtB->uvw0[1] = 0.0;

7583
    _rtB->uvw0[2] = 0.0;

7584


7585
    /* Start for Enabled SubSystem: '<S15>/Subsystem3' */

7586
    ((boolean_T *)ssGetDWork(S, 614))[0] = false;

7587
    ssSetSolverNeedsReset(S);

7588


7589
    /* Start for If: '<S847>/If' */

7590
    ((int8_T *)ssGetDWork(S, 506))[0] = -1;

7591


7592
    /* Start for IfAction SubSystem: '<S847>/Negative Trace' */

7593
    /* Start for If: '<S877>/Find Maximum Diagonal Value' */

7594
    ((int8_T *)ssGetDWork(S, 510))[0] = -1;

7595


7596
    /* End of Start for SubSystem: '<S847>/Negative Trace' */

7597


7598
    /* Start for If: '<S846>/If' */

7599
    ((int8_T *)ssGetDWork(S, 507))[0] = -1;

7600


7601
    /* Start for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

7602
    AFCS_MODEL1_LMainGearGeometryForcesandMoments_Start(S);

7603


7604
    /* End of Start for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

7605


7606
    /* Start for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

7607
    AFCS_MODEL1_RMainGearGeometryForcesandMoments_Start(S);

7608


7609
    /* End of Start for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

7610


7611
    /* Start for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

7612
    AFCS_MODEL1_NoseGearGeometryForcesandMoments_Start(S);

7613


7614
    /* End of Start for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

7615


7616
    /* Start for If: '<S848>/If' */

7617
    ((int8_T *)ssGetDWork(S, 508))[0] = -1;

7618


7619
    /* Start for IfAction SubSystem: '<S848>/Negative Trace' */

7620
    /* Start for If: '<S902>/Find Maximum Diagonal Value' */

7621
    ((int8_T *)ssGetDWork(S, 509))[0] = -1;

7622


7623
    /* End of Start for SubSystem: '<S848>/Negative Trace' */

7624
    /* End of Start for SubSystem: '<S15>/Subsystem3' */

7625
    /* Start for Enabled SubSystem: '<S299>/Enabled Subsystem' */

7626
    ((boolean_T *)ssGetDWork(S, 613))[0] = false;

7627
    ssSetSolverNeedsReset(S);

7628


7629
    /* End of Start for SubSystem: '<S299>/Enabled Subsystem' */

7630
    /* Start for DataStoreMemory: '<S335>/Data Store Memory' */

7631
    ((uint16_T *)ssGetDWork(S, 351))[0] = 1U;

7632


7633
    /* Start for DataStoreMemory: '<S335>/Data Store Memory1' */

7634
    ((uint32_T *)ssGetDWork(S, 343))[0] = 0U;

7635


7636
    /* Start for DataStoreMemory: '<S335>/Data Store Memory10' */

7637
    memset(&((real_T *)ssGetDWork(S, 12))[0], 0, sizeof(real_T) << 5U);

7638


7639
    /* Start for DataStoreMemory: '<S335>/Data Store Memory5' */

7640
    memset(&((real_T *)ssGetDWork(S, 13))[0], 0, sizeof(real_T) << 5U);

7641


7642
    /* Start for DataStoreMemory: '<S335>/Data Store Memory6' */

7643
    memset(&((real_T *)ssGetDWork(S, 14))[0], 0, sizeof(real_T) << 5U);

7644


7645
    /* Start for DataStoreMemory: '<S335>/Data Store Memory7' */

7646
    memset(&((real_T *)ssGetDWork(S, 15))[0], 0, sizeof(real_T) << 5U);

7647


7648
    /* Start for DataStoreMemory: '<S335>/Data Store Memory8' */

7649
    memset(&((real_T *)ssGetDWork(S, 16))[0], 0, sizeof(real_T) << 5U);

7650


7651
    /* Start for DataStoreMemory: '<S335>/Data Store Memory9' */

7652
    memset(&((real_T *)ssGetDWork(S, 17))[0], 0, sizeof(real_T) << 5U);

7653


7654
    /* End of Start for SubSystem: '<S9>/Trajectory planning,tracking' */

7655
    /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */

7656
    {

7657
      SimStruct *rts = ssGetSFunction(S, 167);

7658
      sfcnStart(rts);

7659
      if (ssGetErrorStatus(rts) != (NULL))

7660
        return;

7661
    }

7662


7663
    /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */

7664
    {

7665
      SimStruct *rts = ssGetSFunction(S, 168);

7666
      sfcnStart(rts);

7667
      if (ssGetErrorStatus(rts) != (NULL))

7668
        return;

7669
    }

7670


7671
    /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */

7672
    {

7673
      SimStruct *rts = ssGetSFunction(S, 169);

7674
      sfcnStart(rts);

7675
      if (ssGetErrorStatus(rts) != (NULL))

7676
        return;

7677
    }

7678


7679
    /* Start for Constant: '<S13>/Constant23' */

7680
    _rtB->EnLowFlag = 0;

7681


7682
    /* Start for SwitchCase: '<S31>/Switch Case' */

7683
    ((int8_T *)ssGetDWork(S, 505))[0] = -1;

7684


7685
    /* Start for Atomic SubSystem: '<S7>/Level' */

7686


7687
    /* Start for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */

7688
    AFCS_MODEL1_CodeReuseSubsystem_Start(S);

7689


7690
    /* End of Start for SubSystem: '<S22>/CodeReuseSubsystem' */

7691


7692
    /* End of Start for SubSystem: '<S7>/Level' */

7693


7694
    /* ConstCode for Outport: '<Root>/PLA_L' incorporates:

7695
     *  Constant: '<S1>/Constant'

7696
     */

7697
    ((real_T *)ssGetOutputPortSignal(S, 20))[0] = 0.0;

7698


7699
    /* ConstCode for Outport: '<Root>/PLA_R' incorporates:

7700
     *  Constant: '<S1>/Constant'

7701
     */

7702
    ((real_T *)ssGetOutputPortSignal(S, 21))[0] = 0.0;

7703


7704
    /* ConstCode for Outport: '<Root>/N2_L' incorporates:

7705
     *  Constant: '<S1>/Constant'

7706
     */

7707
    ((real_T *)ssGetOutputPortSignal(S, 22))[0] = 0.0;

7708


7709
    /* ConstCode for Outport: '<Root>/N2_R' incorporates:

7710
     *  Constant: '<S1>/Constant'

7711
     */

7712
    ((real_T *)ssGetOutputPortSignal(S, 23))[0] = 0.0;

7713


7714
    /* ConstCode for DataTypeConversion: '<S24>/Data Type Conversion46' */

7715
    _rtB->Quaternions[0] = (real32_T)0.0;

7716
    _rtB->Quaternions[1] = (real32_T)0.0;

7717
    _rtB->Quaternions[2] = (real32_T)0.0;

7718
    _rtB->Quaternions[3] = (real32_T)0.0;

7719


7720
    /* Previous zero-crossing state intialization */

7721
    {

7722
      PrevZCX_AFCS_MODEL1_T *_rtZCE;

7723
      _rtZCE = ((PrevZCX_AFCS_MODEL1_T *) _ssGetPrevZCSigState(S));

7724
      _rtZCE->Integrator_Reset_ZCE = UNINITIALIZED_ZCSIG;

7725
      _rtZCE->Integrator_Reset_ZCE_f = UNINITIALIZED_ZCSIG;

7726
    }

7727
  }

7728
}

7729


7730
/* Outputs for root system: '<Root>' */

7731
static void mdlOutputs(SimStruct *S, int_T tid)

7732
{

7733
  static real_T z_ECEF0;

7734
  static real_T x_ECEF0;

7735
  static real_T y_ECEF0;

7736
  static real_T M_END2ECEF[9];

7737
  static real_T Pos_ECEF[3];

7738
  static real_T lata;

7739
  static real_T latb;

7740
  static real_T d;

7741
  static real_T e;

7742
  static real_T f;

7743
  static real_T g;

7744
  static real_T h;

7745
  static real_T j;

7746
  static real_T k;

7747
  static real_T l;

7748
  static real_T m;

7749
  static real_T Pos_NED[3];

7750
  static real_T d_0;

7751
  static real_T e_0;

7752
  static real_T f_0;

7753
  static real_T g_0;

7754
  static real_T h_0;

7755
  static real_T i;

7756
  static real_T j_0;

7757
  static real_T k_0;

7758
  static real_T l_0;

7759
  static real_T m_0;

7760
  static real_T n;

7761
  static real_T o;

7762
  static real_T p;

7763
  static real_T q;

7764
  int8_T kk1;

7765
  static real32_T DXp;

7766
  static real32_T DYp;

7767
  static real32_T COSA;

7768
  static real32_T COSB;

7769
  static real_T DD0;

7770
  static real_T PSDref0;

7771
  static real_T Dp0;

7772
  static real_T PSDp0;

7773
  static real32_T PsdNorthRef;

7774
  static int32_T DireFg;

7775
  static real32_T Xcircl1;

7776
  static real32_T Ycircl1;

7777
  static real32_T Xcircl2;

7778
  static real32_T Ycircl2;

7779
  static real32_T DX;

7780
  static real32_T DY;

7781
  static real32_T DDOC;

7782
  static real32_T PsiOC;

7783
  static real32_T Deta;

7784
  static real32_T alp1;

7785
  static real32_T alp2;

7786
  static real32_T Xtp1;

7787
  static real32_T Ytp1;

7788
  static real32_T Xtp2;

7789
  static real32_T Ytp2;

7790
  static real32_T XnorthTP;

7791
  static real32_T YeastTP;

7792
  static real32_T DD;

7793
  static real32_T PsdNorthD;

7794
  static real32_T Xrw;

7795
  static real32_T Yrw;

7796
  static real32_T b_DD;

7797
  static real32_T b_PsdNorthD;

7798
  static real32_T b_Xrw;

7799
  static real32_T b_Yrw;

7800
  static real32_T b;

7801
  static real32_T c;

7802
  static real32_T d_1;

7803
  static real32_T e_1;

7804
  static real32_T f_1;

7805
  static real32_T g_1;

7806
  static real32_T h_1;

7807
  static real32_T i_0;

7808
  static real32_T j_1;

7809
  static real32_T k_1;

7810
  boolean_T resetIntg;

7811
  boolean_T didZcEventOccur;

7812
  static real_T Lat0;

7813
  static real_T Lon0;

7814
  static real_T psi;

7815
  static real_T waypiont_xnAll[32];

7816
  static real_T waypiont_yeAll[32];

7817
  static real_T waypiont_zhAll[32];

7818
  static real_T waypiont_velAll[32];

7819
  static real_T RminAll[32];

7820
  static real_T RminAllNewCal[255];

7821
  static real_T dclose;

7822
  static real_T pn;

7823
  static real_T pe;

7824
  static real_T pd;

7825
  static real_T R;

7826
  static real_T unusedU2;

7827
  static real_T xq1;

7828
  static real_T yq1;

7829
  static real_T hq1;

7830
  static real_T fis;

7831
  static real_T Lamda1;

7832
  static real_T xq2;

7833
  static real_T yq2;

7834
  static real_T hq2;

7835
  static real_T fi2;

7836
  static real_T Lamda2;

7837
  static real_T x0_dot;

7838
  static real_T y0_dot;

7839
  static real_T tx_dot;

7840
  static real_T ty_dot;

7841
  static real_T fi1sum;

7842
  static real_T fi2sum;

7843
  static real_T b_R;

7844
  static real_T unusedU1;

7845
  static real_T b_xq1;

7846
  static real_T b_yq1;

7847
  static real_T b_hq1;

7848
  static real_T b_fis;

7849
  static real_T b_Lamda1;

7850
  static real_T b_xq2;

7851
  static real_T b_yq2;

7852
  static real_T b_hq2;

7853
  static real_T b_fi2;

7854
  static real_T b_Lamda2;

7855
  static real_T b_x0_dot;

7856
  static real_T b_y0_dot;

7857
  static real_T b_tx_dot;

7858
  static real_T b_ty_dot;

7859
  static real_T b_fi1sum;

7860
  static real_T b_fi2sum;

7861
  static real_T vx_qdout[255];

7862
  static real_T vy_qdout[255];

7863
  static real_T xn_qdout[255];

7864
  static real_T ye_qdout[255];

7865
  static real_T zh_qdout[255];

7866
  static real_T b_0;

7867
  static real_T c_0;

7868
  static real_T unusedU0;

7869
  static real_T B;

7870
  static real_T e_2;

7871
  static real_T psic;

7872
  static real_T qsn;

7873
  static real_T qse;

7874
  static real_T dR;

7875
  static int32_T flag;

7876
  static real_T qn;

7877
  static real_T qe;

7878
  static real_T dps;

7879
  static real_T dpl;

7880
  static real_T Vg;

7881
  static real_T flpsic;

7882
  static real_T epy;

7883
  static real_T unusedU1_0;

7884
  static real_T fopsic;

7885
  static real_T acmd;

7886
  static real_T unusedU0_0;

7887
  static real_T b_fopsic;

7888
  static real_T b_acmd;

7889
  static real_T b_Vg;

7890
  static real_T L1;

7891
  static real_T Ru;

7892
  static real_T psiq;

7893
  static real_T delt;

7894
  static real_T R_0;

7895
  static real_T xt;

7896
  static real_T yt;

7897
  static real_T eta;

7898
  static real_T T;

7899
  int8_T rtAction;

7900
  static real_T TrigonometricFunction4_o;

7901
  static real_T j_2[9];

7902
  static real_T lata_0[9];

7903
  static int32_T i_1;

7904
  static real_T tmp[6];

7905
  static real_T tmp_0[6];

7906
  static real_T d_2[9];

7907
  static real_T h_2[9];

7908
  static real_T x_ECEF0_0[3];

7909
  static real_T rateLimiterRate;

7910
  static real_T postoNextqd_idx_1;

7911
  static real_T postoNextqd_idx_0;

7912
  static real_T tmpForInput_idx_0;

7913
  static real_T tmpForInput_idx_2;

7914
  static real_T tmpForInput_idx_1;

7915
  static int64_T tmp_1;

7916
  static uint32_T tmp_2;

7917
  static uint32_T tmp_3;

7918
  static uint32_T tmp_4;

7919
  static uint32_T tmp_5;

7920
  static uint32_T tmp_6;

7921
  static uint32_T tmp_7;

7922
  static uint32_T tmp_8;

7923
  uint32_T tmp_9;

7924
  uint32_T qY;

7925
  uint32_T tmp_a;

7926
  uint32_T tmp_b;

7927
  int32_T tmp_c;

7928
  int32_T tmp_d;

7929
  B_AFCS_MODEL1_T *_rtB;

7930
  X_AFCS_MODEL1_T *_rtX;

7931
  PrevZCX_AFCS_MODEL1_T *_rtZCE;

7932
  _rtZCE = ((PrevZCX_AFCS_MODEL1_T *) _ssGetPrevZCSigState(S));

7933
  _rtX = ((X_AFCS_MODEL1_T *) ssGetContStates(S));

7934
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

7935
  if (1) {

7936
    /* MATLAB Function: '<S1>/MATLAB Function1' incorporates:

7937
     *  Inport: '<Root>/lat0'

7938
     *  Inport: '<Root>/lon0'

7939
     *  Inport: '<Root>/piontNum'

7940
     *  Inport: '<Root>/routedata_NEH'

7941
     */

7942
    /* MATLAB Function 'AFCS_MODEL1/MATLAB Function1': '<S8>:1' */

7943
    /* '<S8>:1:8' */

7944
    /* '<S8>:1:23' */

7945
    /* '<S8>:1:19' */

7946
    /* 输入： */

7947
    /* routedata_NEH: 航路点位置（北东高） */

7948
    /* piontNum：实际航点数 */

7949
    /* Lon0,Lat0,H0:起点的经纬高 */

7950
    /* 输出： */

7951
    /* '<S8>:1:8' */

7952
    /* '<S8>:1:9' */

7953
    /* '<S8>:1:10' */

7954
    /*  h0 = 0; */

7955
    /* '<S8>:1:13' */

7956
    /* '<S8>:1:14' */

7957
    d = *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];

7958
    AFCS_MODEL1_sind_m(&d);

7959
    z_ECEF0 = 6.378137E+6 * d;

7960


7961
    /* '<S8>:1:15' */

7962
    e = *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];

7963
    AFCS_MODEL1_cosd_c(&e);

7964
    f = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];

7965
    AFCS_MODEL1_cosd_c(&f);

7966
    x_ECEF0 = 6.378137E+6 * e * f;

7967


7968
    /* '<S8>:1:16' */

7969
    g = *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];

7970
    AFCS_MODEL1_cosd_c(&g);

7971
    h = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];

7972
    AFCS_MODEL1_sind_m(&h);

7973
    y_ECEF0 = 6.378137E+6 * g * h;

7974


7975
    /* '<S8>:1:18' */

7976
    /* '<S8>:1:21' */

7977
    /* '<S8>:1:24' */

7978
    /* '<S8>:1:25' */

7979
    j = -90.0 - *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];

7980
    AFCS_MODEL1_cosd_c(&j);

7981
    k = 90.0 + *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];

7982
    AFCS_MODEL1_sind_m(&k);

7983
    l = -90.0 - *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];

7984
    AFCS_MODEL1_sind_m(&l);

7985
    m = -90.0 - *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];

7986
    AFCS_MODEL1_cosd_c(&m);

7987
    lata = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];

7988
    AFCS_MODEL1_cosd_c(&lata);

7989
    latb = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];

7990
    AFCS_MODEL1_sind_m(&latb);

7991
    TrigonometricFunction4_o = -*((const real_T **)ssGetInputPortSignalPtrs(S,

7992
      10))[0];

7993
    AFCS_MODEL1_sind_m(&TrigonometricFunction4_o);

7994
    T = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];

7995
    AFCS_MODEL1_cosd_c(&T);

7996
    j_2[0] = j;

7997
    j_2[3] = 0.0;

7998
    j_2[6] = k;

7999
    j_2[1] = 0.0;

8000
    j_2[4] = 1.0;

8001
    j_2[7] = 0.0;

8002
    j_2[2] = l;

8003
    j_2[5] = 0.0;

8004
    j_2[8] = m;

8005
    lata_0[0] = lata;

8006
    lata_0[3] = latb;

8007
    lata_0[6] = 0.0;

8008
    lata_0[1] = TrigonometricFunction4_o;

8009
    lata_0[4] = T;

8010
    lata_0[7] = 0.0;

8011
    lata_0[2] = 0.0;

8012
    lata_0[5] = 0.0;

8013
    lata_0[8] = 1.0;

8014
    for (i_1 = 0; i_1 < 3; i_1++) {

8015
      for (flag = 0; flag < 3; flag++) {

8016
        M_END2ECEF[i_1 + 3 * flag] = 0.0;

8017
        M_END2ECEF[i_1 + 3 * flag] += lata_0[3 * i_1] * j_2[flag];

8018
        M_END2ECEF[i_1 + 3 * flag] += lata_0[3 * i_1 + 1] * j_2[flag + 3];

8019
        M_END2ECEF[i_1 + 3 * flag] += lata_0[3 * i_1 + 2] * j_2[flag + 6];

8020
      }

8021
    }

8022


8023
    /* '<S8>:1:27' */

8024
    /* '<S8>:1:28' */

8025
    memset(&_rtB->route_Lat[0], 0, 30U * sizeof(real_T));

8026
    memset(&_rtB->route_Lon[0], 0, 30U * sizeof(real_T));

8027


8028
    /* '<S8>:1:30' */

8029
    for (flag = 0; flag < (int32_T)*((const real_T **)ssGetInputPortSignalPtrs(S,

8030
           9))[0]; flag++) {

8031
      /* '<S8>:1:30' */

8032
      /* '<S8>:1:31' */

8033
      x_ECEF0_0[0] = x_ECEF0;

8034
      x_ECEF0_0[1] = y_ECEF0;

8035
      x_ECEF0_0[2] = z_ECEF0;

8036
      for (i_1 = 0; i_1 < 3; i_1++) {

8037
        Pos_ECEF[i_1] = (-*((const real_T **)ssGetInputPortSignalPtrs(S, 8))[3 *

8038
                         flag + 2] * M_END2ECEF[i_1 + 6] + (*((const real_T **)

8039
          ssGetInputPortSignalPtrs(S, 8))[3 * flag + 1] * M_END2ECEF[i_1 + 3] + *

8040
          ((const real_T **)ssGetInputPortSignalPtrs(S, 8))[3 * flag] *

8041
          M_END2ECEF[i_1])) + x_ECEF0_0[i_1];

8042
      }

8043


8044
      /* '<S8>:1:32' */

8045
      _rtB->route_Lat[flag] = asin(Pos_ECEF[2] / (sqrt((Pos_ECEF[0] * Pos_ECEF[0]

8046
        + Pos_ECEF[1] * Pos_ECEF[1]) + Pos_ECEF[2] * Pos_ECEF[2]) + 1.0E-6)) *

8047
        57.295779513082323;

8048


8049
      /* '<S8>:1:33' */

8050
      _rtB->route_Lon[flag] = asin(Pos_ECEF[1] / (sqrt(Pos_ECEF[0] * Pos_ECEF[0]

8051
        + Pos_ECEF[1] * Pos_ECEF[1]) + 1.0E-6)) * 57.295779513082323;

8052


8053
      /* '<S8>:1:30' */

8054
    }

8055


8056
    /* '<S8>:1:35' */

8057
    /* 由初始两个航点计算初始偏航角 */

8058
    /* '<S8>:1:39' */

8059
    /* '<S8>:1:41' */

8060
    /* '<S8>:1:42' */

8061
    lata = _rtB->route_Lat[0] * 0.017453292519943295;

8062


8063
    /* '<S8>:1:43' */

8064
    /* '<S8>:1:44' */

8065
    latb = _rtB->route_Lat[1] * 0.017453292519943295;

8066


8067
    /* %%%%%%%%%%%%%%%计算AB与北向夹角 */

8068
    /* '<S8>:1:47' */

8069
    /* '<S8>:1:48' */

8070
    /* '<S8>:1:49' */

8071
    /*  y = u; */

8072
    _rtB->PsiNorth = AFCS_MODEL1_rt_atan2d_snf((_rtB->route_Lon[1] *

8073
      0.017453292519943295 - _rtB->route_Lon[0] * 0.017453292519943295) * cos

8074
      ((latb + lata) * 0.5), latb - lata);

8075
    for (i_1 = 0; i_1 < 30; i_1++) {

8076
      _rtB->route_alt[i_1] = *((const real_T **)ssGetInputPortSignalPtrs(S, 8))

8077
        [3 * i_1 + 2];

8078


8079
      /* DataTypeConversion: '<S17>/Data Type Conversion' incorporates:

8080
       *  Inport: '<Root>/routedata_NEH'

8081
       */

8082
      _rtB->routeAlt[i_1] = (real32_T)_rtB->route_alt[i_1];

8083
    }

8084


8085
    /* End of MATLAB Function: '<S1>/MATLAB Function1' */

8086


8087
    /* DataTypeConversion: '<S17>/Data Type Conversion3' incorporates:

8088
     *  Inport: '<Root>/piontNum'

8089
     */

8090
    TrigonometricFunction4_o = floor(*((const real_T **)ssGetInputPortSignalPtrs

8091
      (S, 9))[0]);

8092
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

8093
    {

8094
      TrigonometricFunction4_o = 0.0;

8095
    } else {

8096
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);

8097
    }

8098


8099
    _rtB->NumPoint = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)(int8_T)

8100
      -(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)(uint8_T)

8101
      TrigonometricFunction4_o);

8102


8103
    /* End of DataTypeConversion: '<S17>/Data Type Conversion3' */

8104


8105
    /* MATLAB Function: '<S9>/MATLAB Function1' incorporates:

8106
     *  Constant: '<S17>/Constant6'

8107
     */

8108
    /* MATLAB Function 'AFCS_MODEL1/PlanningSystem_LLA/MATLAB Function1': '<S333>:1' */

8109
    /* '<S333>:1:5' */

8110
    memset(&_rtB->A[0], 0, 120U * sizeof(real_T));

8111


8112
    /* '<S333>:1:7' */

8113
    for (rtAction = 1; rtAction <= _rtB->NumPoint; rtAction++) {

8114
      /* '<S333>:1:7' */

8115
      /* '<S333>:1:9' */

8116
      i_1 = rtAction - 1;

8117
      _rtB->A[i_1 << 2] = _rtB->route_Lon[rtAction - 1];

8118
      _rtB->A[1 + (i_1 << 2)] = _rtB->route_Lat[rtAction - 1];

8119
      _rtB->A[2 + (i_1 << 2)] = _rtB->routeAlt[rtAction - 1];

8120
      _rtB->A[3 + (i_1 << 2)] = 40.0;

8121
    }

8122


8123
    /* '<S333>:1:12' */

8124
    _rtB->LLA0[0] = _rtB->route_Lat[0];

8125
    _rtB->LLA0[1] = _rtB->route_Lon[0];

8126
    _rtB->LLA0[2] = _rtB->routeAlt[0];

8127


8128
    /* End of MATLAB Function: '<S9>/MATLAB Function1' */

8129


8130
    /* Step: '<S1>/takeoff' */

8131
    /* 纬度，经度，高度 */

8132
    if (ssGetTaskTime(S,0) < 1.0) {

8133
      _rtB->u2u3_a = 0.0;

8134
    } else {

8135
      _rtB->u2u3_a = 10.0;

8136
    }

8137


8138
    /* End of Step: '<S1>/takeoff' */

8139


8140
    /* RelationalOperator: '<S5>/Compare' incorporates:

8141
     *  Constant: '<S5>/Constant'

8142
     */

8143
    _rtB->LogicalOperator1_b = (_rtB->u2u3_a == 10.0);

8144


8145
    /* DataTypeConversion: '<S14>/Data Type Conversion' */

8146
    _rtB->takeoff = _rtB->LogicalOperator1_b;

8147


8148
    /* Gain: '<S14>/Gain' */

8149
    _rtB->Switch_dn = 57.3 * _rtB->PsiNorth;

8150


8151
    /* DataTypeConversion: '<S14>/Data Type Conversion34' */

8152
    _rtB->psi_ini = (real32_T)_rtB->Switch_dn;

8153
  }

8154


8155
  if (ssIsSampleHit(S, 1, 0)) {

8156
    /* Memory: '<S433>/Memory10' */

8157
    _rtB->Memory10 = ((real32_T *)ssGetDWork(S, 270))[0];

8158


8159
    /* Memory: '<S438>/Memory1' */

8160
    _rtB->Memory1 = ((real32_T *)ssGetDWork(S, 271))[0];

8161


8162
    /* Memory: '<S437>/Memory1' */

8163
    _rtB->Memory1_k = ((real32_T *)ssGetDWork(S, 272))[0];

8164


8165
    /* Memory: '<S436>/Memory1' */

8166
    _rtB->Memory1_f = ((real32_T *)ssGetDWork(S, 273))[0];

8167


8168
    /* Memory: '<S435>/Memory1' */

8169
    _rtB->Memory1_p = ((real32_T *)ssGetDWork(S, 274))[0];

8170


8171
    /* Memory: '<S434>/Memory1' */

8172
    _rtB->Memory1_g = ((real32_T *)ssGetDWork(S, 275))[0];

8173


8174
    /* Memory: '<S439>/Memory1' */

8175
    _rtB->Memory1_e = ((real32_T *)ssGetDWork(S, 276))[0];

8176


8177
    /* DataTypeConversion: '<S14>/Data Type Conversion13' incorporates:

8178
     *  Constant: '<S14>/Constant3'

8179
     */

8180
    _rtB->Alt0 = 0.0F;

8181
  }

8182


8183
  /* Outputs for Atomic SubSystem: '<S1>/Subsystem5' */

8184
  if (ssIsSampleHit(S, 1, 0)) {

8185
    /* Gain: '<S15>/Gain3' */

8186
    _rtB->Gain3_be = -_rtB->Memory1_k;

8187


8188
    /* Gain: '<S15>/Gain11' */

8189
    _rtB->Gain11 = _rtB->Alt0;

8190


8191
    /* Constant: '<S440>/Constant3' */

8192
    _rtB->uvw0[0] = 0.0;

8193
    _rtB->uvw0[1] = 0.0;

8194
    _rtB->uvw0[2] = 0.0;

8195


8196
    /* DataTypeConversion: '<S15>/Data Type Conversion1' incorporates:

8197
     *  Constant: '<S14>/Constant18'

8198
     */

8199
    _rtB->DataTypeConversion1 = -1.0;

8200
  }

8201


8202
  if (1) {

8203
    /* Switch: '<S15>/Switch' incorporates:

8204
     *  Constant: '<S447>/Constant1'

8205
     *  Constant: '<S447>/Constant10'

8206
     *  Constant: '<S447>/Constant2'

8207
     *  Constant: '<S447>/Constant3'

8208
     *  Constant: '<S447>/Constant4'

8209
     *  Constant: '<S447>/Constant5'

8210
     *  Constant: '<S447>/Constant6'

8211
     *  Constant: '<S447>/Constant7'

8212
     *  Constant: '<S447>/Constant8'

8213
     *  Constant: '<S447>/Constant9'

8214
     */

8215
    if (_rtB->takeoff > 0) {

8216
      _rtB->Switch_fi[0] = _rtB->Memory10;

8217
      _rtB->Switch_fi[1] = _rtB->Memory1;

8218
      _rtB->Switch_fi[2] = _rtB->Memory1;

8219
      _rtB->Switch_fi[3] = _rtB->Memory1_k;

8220
      _rtB->Switch_fi[4] = _rtB->Gain3_be;

8221
      _rtB->Switch_fi[5] = _rtB->Memory1_f;

8222
      _rtB->Switch_fi[6] = _rtB->Memory1_p;

8223
      _rtB->Switch_fi[7] = _rtB->Memory1_g;

8224
      _rtB->Switch_fi[8] = _rtB->Memory1_e;

8225
      _rtB->Switch_fi[9] = _rtB->Memory1_e;

8226
    } else {

8227
      _rtB->Switch_fi[0] = 0.0F;

8228
      _rtB->Switch_fi[1] = 0.0F;

8229
      _rtB->Switch_fi[2] = 0.0F;

8230
      _rtB->Switch_fi[3] = 0.0F;

8231
      _rtB->Switch_fi[4] = 0.0F;

8232
      _rtB->Switch_fi[5] = 0.0F;

8233
      _rtB->Switch_fi[6] = 0.0F;

8234
      _rtB->Switch_fi[7] = 0.0F;

8235
      _rtB->Switch_fi[8] = 0.0F;

8236
      _rtB->Switch_fi[9] = 0.0F;

8237
    }

8238


8239
    /* End of Switch: '<S15>/Switch' */

8240


8241
    /* DataTypeConversion: '<S15>/Data Type Conversion4' */

8242
    for (i_1 = 0; i_1 < 10; i_1++) {

8243
      _rtB->DataTypeConversion4[i_1] = _rtB->Switch_fi[i_1];

8244
    }

8245


8246
    /* End of DataTypeConversion: '<S15>/Data Type Conversion4' */

8247


8248
    /* DataTypeConversion: '<S15>/Data Type Conversion' */

8249
    _rtB->DataTypeConversion = _rtB->psi_ini;

8250


8251
    /* Gain: '<S442>/Unit Conversion' */

8252
    _rtB->UnitConversion = 0.017453292519943295 * _rtB->DataTypeConversion;

8253


8254
    /* RelationalOperator: '<S443>/Compare' incorporates:

8255
     *  Constant: '<S443>/Constant'

8256
     */

8257
    _rtB->Compare_h = (_rtB->takeoff == 1);

8258
  }

8259


8260
  if (ssIsSampleHit(S, 1, 0)) {

8261
    /* SignalConversion: '<S15>/HiddenBuf_InsertedFor_Subsystem3_at_inport_8' */

8262
    _rtB->HiddenBuf_InsertedFor_Subsystem = _rtB->Compare_h;

8263


8264
    /* Outputs for Enabled SubSystem: '<S15>/Subsystem3' incorporates:

8265
     *  EnablePort: '<S446>/Enable'

8266
     */

8267
    if (ssIsMajorTimeStep(S)) {

8268
      if (_rtB->HiddenBuf_InsertedFor_Subsystem) {

8269
        if (!((boolean_T *)ssGetDWork(S, 614))[0]) {

8270
          /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:

8271
           *  EnablePort: '<S651>/Enable'

8272
           */

8273
          if (ssGetTaskTime(S,1) != ssGetTStart(S)) {

8274
            ssSetSolverNeedsReset(S);

8275
          }

8276


8277
          /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */

8278
          ((boolean_T *)ssGetDWork(S, 614))[0] = true;

8279
        }

8280
      } else {

8281
        if (((boolean_T *)ssGetDWork(S, 614))[0]) {

8282
          ssSetSolverNeedsReset(S);

8283


8284
          /* Disable for If: '<S847>/If' */

8285
          ((int8_T *)ssGetDWork(S, 506))[0] = -1;

8286


8287
          /* Disable for If: '<S846>/If' */

8288
          ((int8_T *)ssGetDWork(S, 507))[0] = -1;

8289


8290
          /* Disable for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

8291
          AFCS_MODEL1_LMainGearGeometryForcesandMoments_Disable(S,

8292
            &_rtB->LMainGearGeometryForcesandMomen);

8293


8294
          /* End of Disable for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

8295


8296
          /* Disable for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

8297
          AFCS_MODEL1_RMainGearGeometryForcesandMoments_Disable(S,

8298
            &_rtB->RMainGearGeometryForcesandMomen);

8299


8300
          /* End of Disable for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

8301


8302
          /* Disable for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

8303
          AFCS_MODEL1_NoseGearGeometryForcesandMoments_Disable(S,

8304
            &_rtB->NoseGearGeometryForcesandMoment);

8305


8306
          /* End of Disable for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

8307


8308
          /* Disable for If: '<S848>/If' */

8309
          ((int8_T *)ssGetDWork(S, 508))[0] = -1;

8310
          ((boolean_T *)ssGetDWork(S, 614))[0] = false;

8311
        }

8312
      }

8313
    }

8314


8315
    /* End of Outputs for SubSystem: '<S15>/Subsystem3' */

8316
  }

8317


8318
  /* Outputs for Enabled SubSystem: '<S15>/Subsystem3' incorporates:

8319
   *  EnablePort: '<S446>/Enable'

8320
   */

8321
  if (((boolean_T *)ssGetDWork(S, 614))[0]) {

8322
    if (1) {

8323
      /* Integrator: '<S472>/Servo rate' */

8324
      _rtB->Servorate = _rtX->Servorate_CSTATE;

8325


8326
      /* Gain: '<S472>/2*zeta * wn' */

8327
      _rtB->table2D_Cm_dr = 35.0 * _rtB->Servorate;

8328


8329
      /* Saturate: '<S472>/Demand limits' */

8330
      if (_rtB->DataTypeConversion4[1] > 30.0) {

8331
        _rtB->saturation = 30.0;

8332
      } else if (_rtB->DataTypeConversion4[1] < -30.0) {

8333
        _rtB->saturation = -30.0;

8334
      } else {

8335
        _rtB->saturation = _rtB->DataTypeConversion4[1];

8336
      }

8337


8338
      /* End of Saturate: '<S472>/Demand limits' */

8339


8340
      /* Integrator: '<S472>/Limited Deflection' */

8341
      /* Limited  Integrator  */

8342
      if (_rtX->LimitedDeflection_CSTATE >= 30.0) {

8343
        if (_rtX->LimitedDeflection_CSTATE != 30.0) {

8344
          _rtX->LimitedDeflection_CSTATE = 30.0;

8345
          ssSetSolverNeedsReset(S);

8346
        }

8347
      } else {

8348
        if ((_rtX->LimitedDeflection_CSTATE <= -30.0) &&

8349
            (_rtX->LimitedDeflection_CSTATE != -30.0)) {

8350
          _rtX->LimitedDeflection_CSTATE = -30.0;

8351
          ssSetSolverNeedsReset(S);

8352
        }

8353
      }

8354


8355
      _rtB->table2D_Cm_dfar = _rtX->LimitedDeflection_CSTATE;

8356


8357
      /* End of Integrator: '<S472>/Limited Deflection' */

8358


8359
      /* Sum: '<S472>/Sum3' */

8360
      _rtB->saturation -= _rtB->table2D_Cm_dfar;

8361


8362
      /* Saturate: '<S472>/rate limit*2*zeta//wn' */

8363
      if (_rtB->saturation > 16.8) {

8364
        _rtB->saturation = 16.8;

8365
      } else {

8366
        if (_rtB->saturation < -16.8) {

8367
          _rtB->saturation = -16.8;

8368
        }

8369
      }

8370


8371
      /* End of Saturate: '<S472>/rate limit*2*zeta//wn' */

8372


8373
      /* Gain: '<S472>/wn^2' */

8374
      _rtB->saturation *= 625.0;

8375


8376
      /* Sum: '<S472>/Sum2' */

8377
      _rtB->Sum2 = _rtB->saturation - _rtB->table2D_Cm_dr;

8378


8379
      /* Integrator: '<S473>/Servo rate' */

8380
      _rtB->Servorate_l = _rtX->Servorate_CSTATE_n;

8381


8382
      /* Gain: '<S473>/2*zeta * wn' */

8383
      _rtB->saturation = 35.0 * _rtB->Servorate_l;

8384


8385
      /* Saturate: '<S473>/Demand limits' */

8386
      if (_rtB->DataTypeConversion4[2] > 30.0) {

8387
        _rtB->table2D_Cm_dr = 30.0;

8388
      } else if (_rtB->DataTypeConversion4[2] < -30.0) {

8389
        _rtB->table2D_Cm_dr = -30.0;

8390
      } else {

8391
        _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[2];

8392
      }

8393


8394
      /* End of Saturate: '<S473>/Demand limits' */

8395


8396
      /* Integrator: '<S473>/Limited Deflection' */

8397
      /* Limited  Integrator  */

8398
      if (_rtX->LimitedDeflection_CSTATE_i >= 30.0) {

8399
        if (_rtX->LimitedDeflection_CSTATE_i != 30.0) {

8400
          _rtX->LimitedDeflection_CSTATE_i = 30.0;

8401
          ssSetSolverNeedsReset(S);

8402
        }

8403
      } else {

8404
        if ((_rtX->LimitedDeflection_CSTATE_i <= -30.0) &&

8405
            (_rtX->LimitedDeflection_CSTATE_i != -30.0)) {

8406
          _rtX->LimitedDeflection_CSTATE_i = -30.0;

8407
          ssSetSolverNeedsReset(S);

8408
        }

8409
      }

8410


8411
      _rtB->table2D_Cn_dfal = _rtX->LimitedDeflection_CSTATE_i;

8412


8413
      /* End of Integrator: '<S473>/Limited Deflection' */

8414


8415
      /* Sum: '<S473>/Sum3' */

8416
      _rtB->table2D_Cm_dr -= _rtB->table2D_Cn_dfal;

8417


8418
      /* Saturate: '<S473>/rate limit*2*zeta//wn' */

8419
      if (_rtB->table2D_Cm_dr > 16.8) {

8420
        _rtB->table2D_Cm_dr = 16.8;

8421
      } else {

8422
        if (_rtB->table2D_Cm_dr < -16.8) {

8423
          _rtB->table2D_Cm_dr = -16.8;

8424
        }

8425
      }

8426


8427
      /* End of Saturate: '<S473>/rate limit*2*zeta//wn' */

8428


8429
      /* Gain: '<S473>/wn^2' */

8430
      _rtB->table2D_Cm_dr *= 625.0;

8431


8432
      /* Sum: '<S473>/Sum2' */

8433
      _rtB->Sum2_o = _rtB->table2D_Cm_dr - _rtB->saturation;

8434


8435
      /* Integrator: '<S474>/Servo rate' */

8436
      _rtB->Servorate_lu = _rtX->Servorate_CSTATE_m;

8437


8438
      /* Gain: '<S474>/2*zeta * wn' */

8439
      _rtB->saturation = 35.0 * _rtB->Servorate_lu;

8440


8441
      /* Saturate: '<S474>/Demand limits' */

8442
      if (_rtB->DataTypeConversion4[3] > 30.0) {

8443
        _rtB->table2D_Cm_dr = 30.0;

8444
      } else if (_rtB->DataTypeConversion4[3] < -30.0) {

8445
        _rtB->table2D_Cm_dr = -30.0;

8446
      } else {

8447
        _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[3];

8448
      }

8449


8450
      /* End of Saturate: '<S474>/Demand limits' */

8451


8452
      /* Integrator: '<S474>/Limited Deflection' */

8453
      /* Limited  Integrator  */

8454
      if (_rtX->LimitedDeflection_CSTATE_m >= 30.0) {

8455
        if (_rtX->LimitedDeflection_CSTATE_m != 30.0) {

8456
          _rtX->LimitedDeflection_CSTATE_m = 30.0;

8457
          ssSetSolverNeedsReset(S);

8458
        }

8459
      } else {

8460
        if ((_rtX->LimitedDeflection_CSTATE_m <= -30.0) &&

8461
            (_rtX->LimitedDeflection_CSTATE_m != -30.0)) {

8462
          _rtX->LimitedDeflection_CSTATE_m = -30.0;

8463
          ssSetSolverNeedsReset(S);

8464
        }

8465
      }

8466


8467
      _rtB->table2D_Cn_dr = _rtX->LimitedDeflection_CSTATE_m;

8468


8469
      /* End of Integrator: '<S474>/Limited Deflection' */

8470


8471
      /* Sum: '<S474>/Sum3' */

8472
      _rtB->table2D_Cm_dr -= _rtB->table2D_Cn_dr;

8473


8474
      /* Saturate: '<S474>/rate limit*2*zeta//wn' */

8475
      if (_rtB->table2D_Cm_dr > 16.8) {

8476
        _rtB->table2D_Cm_dr = 16.8;

8477
      } else {

8478
        if (_rtB->table2D_Cm_dr < -16.8) {

8479
          _rtB->table2D_Cm_dr = -16.8;

8480
        }

8481
      }

8482


8483
      /* End of Saturate: '<S474>/rate limit*2*zeta//wn' */

8484


8485
      /* Gain: '<S474>/wn^2' */

8486
      _rtB->table2D_Cm_dr *= 625.0;

8487


8488
      /* Sum: '<S474>/Sum2' */

8489
      _rtB->Sum2_f = _rtB->table2D_Cm_dr - _rtB->saturation;

8490


8491
      /* Integrator: '<S475>/Servo rate' */

8492
      _rtB->Servorate_g = _rtX->Servorate_CSTATE_mi;

8493


8494
      /* Gain: '<S475>/2*zeta * wn' */

8495
      _rtB->saturation = 35.0 * _rtB->Servorate_g;

8496


8497
      /* Saturate: '<S475>/Demand limits' */

8498
      if (_rtB->DataTypeConversion4[4] > 30.0) {

8499
        _rtB->table2D_Cm_dr = 30.0;

8500
      } else if (_rtB->DataTypeConversion4[4] < -30.0) {

8501
        _rtB->table2D_Cm_dr = -30.0;

8502
      } else {

8503
        _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[4];

8504
      }

8505


8506
      /* End of Saturate: '<S475>/Demand limits' */

8507


8508
      /* Integrator: '<S475>/Limited Deflection' */

8509
      /* Limited  Integrator  */

8510
      if (_rtX->LimitedDeflection_CSTATE_f >= 30.0) {

8511
        if (_rtX->LimitedDeflection_CSTATE_f != 30.0) {

8512
          _rtX->LimitedDeflection_CSTATE_f = 30.0;

8513
          ssSetSolverNeedsReset(S);

8514
        }

8515
      } else {

8516
        if ((_rtX->LimitedDeflection_CSTATE_f <= -30.0) &&

8517
            (_rtX->LimitedDeflection_CSTATE_f != -30.0)) {

8518
          _rtX->LimitedDeflection_CSTATE_f = -30.0;

8519
          ssSetSolverNeedsReset(S);

8520
        }

8521
      }

8522


8523
      _rtB->table2D_Cn_dar = _rtX->LimitedDeflection_CSTATE_f;

8524


8525
      /* End of Integrator: '<S475>/Limited Deflection' */

8526


8527
      /* Sum: '<S475>/Sum3' */

8528
      _rtB->table2D_Cm_dr -= _rtB->table2D_Cn_dar;

8529


8530
      /* Saturate: '<S475>/rate limit*2*zeta//wn' */

8531
      if (_rtB->table2D_Cm_dr > 16.8) {

8532
        _rtB->table2D_Cm_dr = 16.8;

8533
      } else {

8534
        if (_rtB->table2D_Cm_dr < -16.8) {

8535
          _rtB->table2D_Cm_dr = -16.8;

8536
        }

8537
      }

8538


8539
      /* End of Saturate: '<S475>/rate limit*2*zeta//wn' */

8540


8541
      /* Gain: '<S475>/wn^2' */

8542
      _rtB->table2D_Cm_dr *= 625.0;

8543


8544
      /* Sum: '<S475>/Sum2' */

8545
      _rtB->Sum2_i = _rtB->table2D_Cm_dr - _rtB->saturation;

8546


8547
      /* Integrator: '<S476>/Servo rate' */

8548
      _rtB->Servorate_m = _rtX->Servorate_CSTATE_f;

8549


8550
      /* Gain: '<S476>/2*zeta * wn' */

8551
      _rtB->saturation = 35.0 * _rtB->Servorate_m;

8552


8553
      /* Saturate: '<S476>/Demand limits' */

8554
      if (_rtB->DataTypeConversion4[5] > 30.0) {

8555
        _rtB->table2D_Cm_dr = 30.0;

8556
      } else if (_rtB->DataTypeConversion4[5] < -30.0) {

8557
        _rtB->table2D_Cm_dr = -30.0;

8558
      } else {

8559
        _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[5];

8560
      }

8561


8562
      /* End of Saturate: '<S476>/Demand limits' */

8563


8564
      /* Integrator: '<S476>/Limited Deflection' */

8565
      /* Limited  Integrator  */

8566
      if (_rtX->LimitedDeflection_CSTATE_g >= 30.0) {

8567
        if (_rtX->LimitedDeflection_CSTATE_g != 30.0) {

8568
          _rtX->LimitedDeflection_CSTATE_g = 30.0;

8569
          ssSetSolverNeedsReset(S);

8570
        }

8571
      } else {

8572
        if ((_rtX->LimitedDeflection_CSTATE_g <= -30.0) &&

8573
            (_rtX->LimitedDeflection_CSTATE_g != -30.0)) {

8574
          _rtX->LimitedDeflection_CSTATE_g = -30.0;

8575
          ssSetSolverNeedsReset(S);

8576
        }

8577
      }

8578


8579
      _rtB->add_e = _rtX->LimitedDeflection_CSTATE_g;

8580


8581
      /* End of Integrator: '<S476>/Limited Deflection' */

8582


8583
      /* Sum: '<S476>/Sum3' */

8584
      _rtB->table2D_Cm_dr -= _rtB->add_e;

8585


8586
      /* Saturate: '<S476>/rate limit*2*zeta//wn' */

8587
      if (_rtB->table2D_Cm_dr > 16.8) {

8588
        _rtB->table2D_Cm_dr = 16.8;

8589
      } else {

8590
        if (_rtB->table2D_Cm_dr < -16.8) {

8591
          _rtB->table2D_Cm_dr = -16.8;

8592
        }

8593
      }

8594


8595
      /* End of Saturate: '<S476>/rate limit*2*zeta//wn' */

8596


8597
      /* Gain: '<S476>/wn^2' */

8598
      _rtB->table2D_Cm_dr *= 625.0;

8599


8600
      /* Sum: '<S476>/Sum2' */

8601
      _rtB->Sum2_p = _rtB->table2D_Cm_dr - _rtB->saturation;

8602


8603
      /* Integrator: '<S477>/Servo rate' */

8604
      _rtB->Servorate_ga = _rtX->Servorate_CSTATE_k;

8605


8606
      /* Gain: '<S477>/2*zeta * wn' */

8607
      _rtB->saturation = 35.0 * _rtB->Servorate_ga;

8608


8609
      /* Saturate: '<S477>/Demand limits' */

8610
      if (_rtB->DataTypeConversion4[6] > 30.0) {

8611
        _rtB->table2D_Cm_dr = 30.0;

8612
      } else if (_rtB->DataTypeConversion4[6] < -30.0) {

8613
        _rtB->table2D_Cm_dr = -30.0;

8614
      } else {

8615
        _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[6];

8616
      }

8617


8618
      /* End of Saturate: '<S477>/Demand limits' */

8619


8620
      /* Integrator: '<S477>/Limited Deflection' */

8621
      /* Limited  Integrator  */

8622
      if (_rtX->LimitedDeflection_CSTATE_l >= 30.0) {

8623
        if (_rtX->LimitedDeflection_CSTATE_l != 30.0) {

8624
          _rtX->LimitedDeflection_CSTATE_l = 30.0;

8625
          ssSetSolverNeedsReset(S);

8626
        }

8627
      } else {

8628
        if ((_rtX->LimitedDeflection_CSTATE_l <= -30.0) &&

8629
            (_rtX->LimitedDeflection_CSTATE_l != -30.0)) {

8630
          _rtX->LimitedDeflection_CSTATE_l = -30.0;

8631
          ssSetSolverNeedsReset(S);

8632
        }

8633
      }

8634


8635
      _rtB->Gain6_k = _rtX->LimitedDeflection_CSTATE_l;

8636


8637
      /* End of Integrator: '<S477>/Limited Deflection' */

8638


8639
      /* Sum: '<S477>/Sum3' */

8640
      _rtB->table2D_Cm_dr -= _rtB->Gain6_k;

8641


8642
      /* Saturate: '<S477>/rate limit*2*zeta//wn' */

8643
      if (_rtB->table2D_Cm_dr > 16.8) {

8644
        _rtB->table2D_Cm_dr = 16.8;

8645
      } else {

8646
        if (_rtB->table2D_Cm_dr < -16.8) {

8647
          _rtB->table2D_Cm_dr = -16.8;

8648
        }

8649
      }

8650


8651
      /* End of Saturate: '<S477>/rate limit*2*zeta//wn' */

8652


8653
      /* Gain: '<S477>/wn^2' */

8654
      _rtB->table2D_Cm_dr *= 625.0;

8655


8656
      /* Sum: '<S477>/Sum2' */

8657
      _rtB->Sum2_m = _rtB->table2D_Cm_dr - _rtB->saturation;

8658


8659
      /* Integrator: '<S478>/Servo rate' */

8660
      _rtB->Servorate_d = _rtX->Servorate_CSTATE_h;

8661


8662
      /* Gain: '<S478>/2*zeta * wn' */

8663
      _rtB->Gain6_k = 210.0 * _rtB->Servorate_d;

8664


8665
      /* Saturate: '<S478>/Demand limits' */

8666
      if (_rtB->DataTypeConversion4[8] > 30.0) {

8667
        _rtB->saturation = 30.0;

8668
      } else if (_rtB->DataTypeConversion4[8] < -30.0) {

8669
        _rtB->saturation = -30.0;

8670
      } else {

8671
        _rtB->saturation = _rtB->DataTypeConversion4[8];

8672
      }

8673


8674
      /* End of Saturate: '<S478>/Demand limits' */

8675


8676
      /* Integrator: '<S478>/Limited Deflection' */

8677
      /* Limited  Integrator  */

8678
      if (_rtX->LimitedDeflection_CSTATE_j >= 30.0) {

8679
        if (_rtX->LimitedDeflection_CSTATE_j != 30.0) {

8680
          _rtX->LimitedDeflection_CSTATE_j = 30.0;

8681
          ssSetSolverNeedsReset(S);

8682
        }

8683
      } else {

8684
        if ((_rtX->LimitedDeflection_CSTATE_j <= -30.0) &&

8685
            (_rtX->LimitedDeflection_CSTATE_j != -30.0)) {

8686
          _rtX->LimitedDeflection_CSTATE_j = -30.0;

8687
          ssSetSolverNeedsReset(S);

8688
        }

8689
      }

8690


8691
      _rtB->table2D_Cm_dr = _rtX->LimitedDeflection_CSTATE_j;

8692


8693
      /* End of Integrator: '<S478>/Limited Deflection' */

8694


8695
      /* Sum: '<S478>/Sum3' */

8696
      _rtB->saturation -= _rtB->table2D_Cm_dr;

8697


8698
      /* Saturate: '<S478>/rate limit*2*zeta//wn' */

8699
      if (_rtB->saturation > 1.4) {

8700
        _rtB->saturation = 1.4;

8701
      } else {

8702
        if (_rtB->saturation < -1.4) {

8703
          _rtB->saturation = -1.4;

8704
        }

8705
      }

8706


8707
      /* End of Saturate: '<S478>/rate limit*2*zeta//wn' */

8708


8709
      /* Gain: '<S478>/wn^2' */

8710
      _rtB->saturation *= 22500.0;

8711


8712
      /* Sum: '<S478>/Sum2' */

8713
      _rtB->Sum2_k = _rtB->saturation - _rtB->Gain6_k;

8714


8715
      /* Integrator: '<S479>/Servo rate' */

8716
      _rtB->Servorate_h = _rtX->Servorate_CSTATE_kx;

8717


8718
      /* Gain: '<S479>/2*zeta * wn' */

8719
      _rtB->Gain6_k = 210.0 * _rtB->Servorate_h;

8720


8721
      /* Saturate: '<S479>/Demand limits' */

8722
      if (_rtB->DataTypeConversion4[9] > 30.0) {

8723
        _rtB->saturation = 30.0;

8724
      } else if (_rtB->DataTypeConversion4[9] < -30.0) {

8725
        _rtB->saturation = -30.0;

8726
      } else {

8727
        _rtB->saturation = _rtB->DataTypeConversion4[9];

8728
      }

8729


8730
      /* End of Saturate: '<S479>/Demand limits' */

8731


8732
      /* Integrator: '<S479>/Limited Deflection' */

8733
      /* Limited  Integrator  */

8734
      if (_rtX->LimitedDeflection_CSTATE_la >= 30.0) {

8735
        if (_rtX->LimitedDeflection_CSTATE_la != 30.0) {

8736
          _rtX->LimitedDeflection_CSTATE_la = 30.0;

8737
          ssSetSolverNeedsReset(S);

8738
        }

8739
      } else {

8740
        if ((_rtX->LimitedDeflection_CSTATE_la <= -30.0) &&

8741
            (_rtX->LimitedDeflection_CSTATE_la != -30.0)) {

8742
          _rtX->LimitedDeflection_CSTATE_la = -30.0;

8743
          ssSetSolverNeedsReset(S);

8744
        }

8745
      }

8746


8747
      _rtB->table2D_Cm_dr = _rtX->LimitedDeflection_CSTATE_la;

8748


8749
      /* End of Integrator: '<S479>/Limited Deflection' */

8750


8751
      /* Sum: '<S479>/Sum3' */

8752
      _rtB->saturation -= _rtB->table2D_Cm_dr;

8753


8754
      /* Saturate: '<S479>/rate limit*2*zeta//wn' */

8755
      if (_rtB->saturation > 1.4) {

8756
        _rtB->saturation = 1.4;

8757
      } else {

8758
        if (_rtB->saturation < -1.4) {

8759
          _rtB->saturation = -1.4;

8760
        }

8761
      }

8762


8763
      /* End of Saturate: '<S479>/rate limit*2*zeta//wn' */

8764


8765
      /* Gain: '<S479>/wn^2' */

8766
      _rtB->saturation *= 22500.0;

8767


8768
      /* Sum: '<S479>/Sum2' */

8769
      _rtB->Sum2_mm = _rtB->saturation - _rtB->Gain6_k;

8770


8771
      /* TransferFcn: '<S462>/TransferFcn1' */

8772
      _rtB->Gain6_k = 0.0;

8773
      _rtB->Gain6_k += 7.1710290426676231 * _rtX->TransferFcn1_CSTATE;

8774


8775
      /* SignalConversion: '<S850>/TmpSignal ConversionAtsincosInport1' incorporates:

8776
       *  Constant: '<S440>/Constant1'

8777
       *  Constant: '<S440>/Constant5'

8778
       */

8779
      _rtB->Product2_f[0] = _rtB->UnitConversion;

8780
      _rtB->Product2_f[1] = 0.0;

8781
      _rtB->Product2_f[2] = 0.0;

8782


8783
      /* Trigonometry: '<S850>/sincos' */

8784
      TrigonometricFunction4_o = cos(_rtB->Product2_f[0]);

8785
      _rtB->Product2_f[0] = sin(_rtB->Product2_f[0]);

8786
      _rtB->ubvbwb[0] = TrigonometricFunction4_o;

8787
      TrigonometricFunction4_o = cos(_rtB->Product2_f[1]);

8788
      _rtB->Product2_f[1] = sin(_rtB->Product2_f[1]);

8789
      _rtB->ubvbwb[1] = TrigonometricFunction4_o;

8790
      TrigonometricFunction4_o = cos(_rtB->Product2_f[2]);

8791
      _rtB->Product2_f[2] = sin(_rtB->Product2_f[2]);

8792
      _rtB->ubvbwb[2] = TrigonometricFunction4_o;

8793


8794
      /* Fcn: '<S850>/Fcn11' */

8795
      _rtB->VectorConcatenate_f[0] = _rtB->ubvbwb[1] * _rtB->ubvbwb[0];

8796


8797
      /* Fcn: '<S850>/Fcn21' */

8798
      _rtB->VectorConcatenate_f[1] = _rtB->Product2_f[2] * _rtB->Product2_f[1] *

8799
        _rtB->ubvbwb[0] - _rtB->ubvbwb[2] * _rtB->Product2_f[0];

8800


8801
      /* Fcn: '<S850>/Fcn31' */

8802
      _rtB->VectorConcatenate_f[2] = _rtB->ubvbwb[2] * _rtB->Product2_f[1] *

8803
        _rtB->ubvbwb[0] + _rtB->Product2_f[2] * _rtB->Product2_f[0];

8804


8805
      /* Fcn: '<S850>/Fcn12' */

8806
      _rtB->VectorConcatenate_f[3] = _rtB->ubvbwb[1] * _rtB->Product2_f[0];

8807


8808
      /* Fcn: '<S850>/Fcn22' */

8809
      _rtB->VectorConcatenate_f[4] = _rtB->Product2_f[2] * _rtB->Product2_f[1] *

8810
        _rtB->Product2_f[0] + _rtB->ubvbwb[2] * _rtB->ubvbwb[0];

8811


8812
      /* Fcn: '<S850>/Fcn32' */

8813
      _rtB->VectorConcatenate_f[5] = _rtB->ubvbwb[2] * _rtB->Product2_f[1] *

8814
        _rtB->Product2_f[0] - _rtB->Product2_f[2] * _rtB->ubvbwb[0];

8815


8816
      /* Fcn: '<S850>/Fcn13' */

8817
      _rtB->VectorConcatenate_f[6] = -_rtB->Product2_f[1];

8818


8819
      /* Fcn: '<S850>/Fcn23' */

8820
      _rtB->VectorConcatenate_f[7] = _rtB->Product2_f[2] * _rtB->ubvbwb[1];

8821


8822
      /* Fcn: '<S850>/Fcn33' */

8823
      _rtB->VectorConcatenate_f[8] = _rtB->ubvbwb[2] * _rtB->ubvbwb[1];

8824
    }

8825


8826
    if (ssIsSampleHit(S, 1, 0)) {

8827
      /* Gain: '<S872>/Unit Conversion' incorporates:

8828
       *  Constant: '<S14>/Constant1'

8829
       *  Constant: '<S14>/Constant2'

8830
       */

8831
      _rtB->sincos_o2_k[0] = 0.69813170079773179;

8832
      _rtB->sincos_o2_k[1] = 0.52359877559829882;

8833


8834
      /* Trigonometry: '<S845>/sincos' */

8835
      TrigonometricFunction4_o = cos(_rtB->sincos_o2_k[0]);

8836
      _rtB->sincos_o2_k[0] = sin(_rtB->sincos_o2_k[0]);

8837
      _rtB->sincos_o1_h[0] = TrigonometricFunction4_o;

8838
      TrigonometricFunction4_o = cos(_rtB->sincos_o2_k[1]);

8839
      _rtB->sincos_o2_k[1] = sin(_rtB->sincos_o2_k[1]);

8840
      _rtB->sincos_o1_h[1] = TrigonometricFunction4_o;

8841


8842
      /* Product: '<S863>/u(1)*u(4)' */

8843
      _rtB->Switch_ly = _rtB->sincos_o2_k[0] * _rtB->sincos_o1_h[1];

8844


8845
      /* UnaryMinus: '<S863>/Unary Minus' */

8846
      _rtB->VectorConcatenate[0] = -_rtB->Switch_ly;

8847


8848
      /* UnaryMinus: '<S866>/Unary Minus' */

8849
      _rtB->VectorConcatenate[1] = -_rtB->sincos_o2_k[1];

8850


8851
      /* Product: '<S869>/u(3)*u(4)' */

8852
      _rtB->Switch_ly = _rtB->sincos_o1_h[0] * _rtB->sincos_o1_h[1];

8853


8854
      /* UnaryMinus: '<S869>/Unary Minus' */

8855
      _rtB->VectorConcatenate[2] = -_rtB->Switch_ly;

8856


8857
      /* Product: '<S864>/u(1)*u(2)' */

8858
      _rtB->Switch_ly = _rtB->sincos_o2_k[0] * _rtB->sincos_o2_k[1];

8859


8860
      /* UnaryMinus: '<S864>/Unary Minus' */

8861
      _rtB->VectorConcatenate[3] = -_rtB->Switch_ly;

8862


8863
      /* SignalConversion: '<S873>/ConcatBufferAtVector ConcatenateIn5' */

8864
      _rtB->VectorConcatenate[4] = _rtB->sincos_o1_h[1];

8865


8866
      /* Product: '<S870>/u(2)*u(3)' */

8867
      _rtB->Switch_ly = _rtB->sincos_o2_k[1] * _rtB->sincos_o1_h[0];

8868


8869
      /* UnaryMinus: '<S870>/Unary Minus' */

8870
      _rtB->VectorConcatenate[5] = -_rtB->Switch_ly;

8871


8872
      /* SignalConversion: '<S873>/ConcatBufferAtVector ConcatenateIn7' */

8873
      _rtB->VectorConcatenate[6] = _rtB->sincos_o1_h[0];

8874


8875
      /* SignalConversion: '<S873>/ConcatBufferAtVector ConcatenateIn8' incorporates:

8876
       *  Constant: '<S868>/Constant'

8877
       */

8878
      _rtB->VectorConcatenate[7] = 0.0;

8879


8880
      /* UnaryMinus: '<S871>/Unary Minus' */

8881
      _rtB->VectorConcatenate[8] = -_rtB->sincos_o2_k[0];

8882
    }

8883


8884
    if (1) {

8885
      /* Product: '<S779>/Product2' */

8886
      for (i_1 = 0; i_1 < 3; i_1++) {

8887
        for (flag = 0; flag < 3; flag++) {

8888
          _rtB->Product2[i_1 + 3 * flag] = 0.0;

8889
          _rtB->Product2[i_1 + 3 * flag] += _rtB->VectorConcatenate[3 * flag] *

8890
            _rtB->VectorConcatenate_f[i_1];

8891
          _rtB->Product2[i_1 + 3 * flag] += _rtB->VectorConcatenate[3 * flag + 1]

8892
            * _rtB->VectorConcatenate_f[i_1 + 3];

8893
          _rtB->Product2[i_1 + 3 * flag] += _rtB->VectorConcatenate[3 * flag + 2]

8894
            * _rtB->VectorConcatenate_f[i_1 + 6];

8895
        }

8896
      }

8897


8898
      /* End of Product: '<S779>/Product2' */

8899


8900
      /* Sum: '<S879>/Add' */

8901
      _rtB->Add = (_rtB->Product2[0] + _rtB->Product2[4]) + _rtB->Product2[8];

8902


8903
      /* If: '<S847>/If' */

8904
      if (ssIsMajorTimeStep(S)) {

8905
        rtAction = (int8_T)!(_rtB->Add > 0.0);

8906
        ((int8_T *)ssGetDWork(S, 506))[0] = rtAction;

8907
      } else {

8908
        rtAction = ((int8_T *)ssGetDWork(S, 506))[0];

8909
      }

8910


8911
      switch (rtAction) {

8912
       case 0:

8913
        /* Outputs for IfAction SubSystem: '<S847>/Positive Trace' incorporates:

8914
         *  ActionPort: '<S878>/Action Port'

8915
         */

8916
        /* Sum: '<S878>/Sum' incorporates:

8917
         *  Constant: '<S878>/Constant'

8918
         */

8919
        _rtB->Gain1_b = _rtB->Add + 1.0;

8920


8921
        /* Sqrt: '<S878>/sqrt' */

8922
        _rtB->Gain1_b = sqrt(_rtB->Gain1_b);

8923


8924
        /* Gain: '<S878>/Gain' */

8925
        _rtB->Merge[0] = 0.5 * _rtB->Gain1_b;

8926


8927
        /* Gain: '<S878>/Gain1' */

8928
        _rtB->Gain1_b *= 2.0;

8929


8930
        /* Sum: '<S900>/Add' */

8931
        _rtB->Add_hw = _rtB->Product2[7] - _rtB->Product2[5];

8932


8933
        /* Sum: '<S899>/Add' */

8934
        _rtB->Add_m = _rtB->Product2[2] - _rtB->Product2[6];

8935


8936
        /* Sum: '<S901>/Add' */

8937
        _rtB->Add_pk = _rtB->Product2[3] - _rtB->Product2[1];

8938


8939
        /* Product: '<S878>/Product' */

8940
        _rtB->Merge[1] = _rtB->Add_hw / _rtB->Gain1_b;

8941
        _rtB->Merge[2] = _rtB->Add_m / _rtB->Gain1_b;

8942
        _rtB->Merge[3] = _rtB->Add_pk / _rtB->Gain1_b;

8943


8944
        /* End of Outputs for SubSystem: '<S847>/Positive Trace' */

8945
        break;

8946


8947
       case 1:

8948
        /* Outputs for IfAction SubSystem: '<S847>/Negative Trace' incorporates:

8949
         *  ActionPort: '<S877>/Action Port'

8950
         */

8951
        /* If: '<S877>/Find Maximum Diagonal Value' */

8952
        if (ssIsMajorTimeStep(S)) {

8953
          if ((_rtB->Product2[4] > _rtB->Product2[0]) && (_rtB->Product2[4] >

8954
               _rtB->Product2[8])) {

8955
            rtAction = 0;

8956
          } else if (_rtB->Product2[8] > _rtB->Product2[0]) {

8957
            rtAction = 1;

8958
          } else {

8959
            rtAction = 2;

8960
          }

8961


8962
          ((int8_T *)ssGetDWork(S, 510))[0] = rtAction;

8963
        } else {

8964
          rtAction = ((int8_T *)ssGetDWork(S, 510))[0];

8965
        }

8966


8967
        switch (rtAction) {

8968
         case 0:

8969
          /* Outputs for IfAction SubSystem: '<S877>/Maximum Value at DCM(2,2)' incorporates:

8970
           *  ActionPort: '<S881>/Action Port'

8971
           */

8972
          /* Sum: '<S893>/Add' incorporates:

8973
           *  Constant: '<S893>/Constant'

8974
           */

8975
          _rtB->Product_co = ((_rtB->Product2[4] - _rtB->Product2[0]) -

8976
                              _rtB->Product2[8]) + 1.0;

8977


8978
          /* Sqrt: '<S881>/sqrt' */

8979
          _rtB->Product_co = sqrt(_rtB->Product_co);

8980


8981
          /* Gain: '<S881>/Gain' */

8982
          _rtB->Merge[2] = 0.5 * _rtB->Product_co;

8983


8984
          /* Sum: '<S891>/Add' */

8985
          _rtB->Add_gh = _rtB->Product2[1] + _rtB->Product2[3];

8986


8987
          /* Sum: '<S890>/Add' */

8988
          _rtB->Add_er = _rtB->Product2[5] + _rtB->Product2[7];

8989


8990
          /* Sum: '<S889>/Add' */

8991
          _rtB->Add_f0 = _rtB->Product2[2] - _rtB->Product2[6];

8992


8993
          /* Switch: '<S892>/Switch' incorporates:

8994
           *  Constant: '<S892>/Constant1'

8995
           */

8996
          if (_rtB->Product_co != 0.0) {

8997
            _rtB->Switch_b5[0] = 0.5;

8998
            _rtB->Switch_b5[1] = _rtB->Product_co;

8999
          } else {

9000
            _rtB->Switch_b5[0] = 0.0;

9001
            _rtB->Switch_b5[1] = 1.0;

9002
          }

9003


9004
          /* End of Switch: '<S892>/Switch' */

9005


9006
          /* Product: '<S892>/Product' */

9007
          _rtB->Product_co = _rtB->Switch_b5[0] / _rtB->Switch_b5[1];

9008


9009
          /* Product: '<S881>/Product' */

9010
          _rtB->Product_e[0] = _rtB->Add_gh * _rtB->Product_co;

9011
          _rtB->Product_e[1] = _rtB->Add_er * _rtB->Product_co;

9012
          _rtB->Product_e[2] = _rtB->Add_f0 * _rtB->Product_co;

9013


9014
          /* Gain: '<S881>/Gain1' */

9015
          _rtB->Merge[1] = _rtB->Product_e[0];

9016


9017
          /* Gain: '<S881>/Gain3' */

9018
          _rtB->Merge[3] = _rtB->Product_e[1];

9019


9020
          /* Gain: '<S881>/Gain4' */

9021
          _rtB->Merge[0] = _rtB->Product_e[2];

9022


9023
          /* End of Outputs for SubSystem: '<S877>/Maximum Value at DCM(2,2)' */

9024
          break;

9025


9026
         case 1:

9027
          /* Outputs for IfAction SubSystem: '<S877>/Maximum Value at DCM(3,3)' incorporates:

9028
           *  ActionPort: '<S882>/Action Port'

9029
           */

9030
          /* Sum: '<S898>/Add' incorporates:

9031
           *  Constant: '<S898>/Constant'

9032
           */

9033
          _rtB->Product_hp = ((_rtB->Product2[8] - _rtB->Product2[0]) -

9034
                              _rtB->Product2[4]) + 1.0;

9035


9036
          /* Sqrt: '<S882>/sqrt' */

9037
          _rtB->Product_hp = sqrt(_rtB->Product_hp);

9038


9039
          /* Gain: '<S882>/Gain' */

9040
          _rtB->Merge[3] = 0.5 * _rtB->Product_hp;

9041


9042
          /* Sum: '<S894>/Add' */

9043
          _rtB->Add_i = _rtB->Product2[2] + _rtB->Product2[6];

9044


9045
          /* Sum: '<S895>/Add' */

9046
          _rtB->Add_kgp = _rtB->Product2[5] + _rtB->Product2[7];

9047


9048
          /* Sum: '<S896>/Add' */

9049
          _rtB->Add_h = _rtB->Product2[3] - _rtB->Product2[1];

9050


9051
          /* Switch: '<S897>/Switch' incorporates:

9052
           *  Constant: '<S897>/Constant1'

9053
           */

9054
          if (_rtB->Product_hp != 0.0) {

9055
            _rtB->Switch_n[0] = 0.5;

9056
            _rtB->Switch_n[1] = _rtB->Product_hp;

9057
          } else {

9058
            _rtB->Switch_n[0] = 0.0;

9059
            _rtB->Switch_n[1] = 1.0;

9060
          }

9061


9062
          /* End of Switch: '<S897>/Switch' */

9063


9064
          /* Product: '<S897>/Product' */

9065
          _rtB->Product_hp = _rtB->Switch_n[0] / _rtB->Switch_n[1];

9066


9067
          /* Product: '<S882>/Product' */

9068
          _rtB->Product_c[0] = _rtB->Add_i * _rtB->Product_hp;

9069
          _rtB->Product_c[1] = _rtB->Add_kgp * _rtB->Product_hp;

9070
          _rtB->Product_c[2] = _rtB->Add_h * _rtB->Product_hp;

9071


9072
          /* Gain: '<S882>/Gain1' */

9073
          _rtB->Merge[1] = _rtB->Product_c[0];

9074


9075
          /* Gain: '<S882>/Gain2' */

9076
          _rtB->Merge[2] = _rtB->Product_c[1];

9077


9078
          /* Gain: '<S882>/Gain3' */

9079
          _rtB->Merge[0] = _rtB->Product_c[2];

9080


9081
          /* End of Outputs for SubSystem: '<S877>/Maximum Value at DCM(3,3)' */

9082
          break;

9083


9084
         case 2:

9085
          /* Outputs for IfAction SubSystem: '<S877>/Maximum Value at DCM(1,1)' incorporates:

9086
           *  ActionPort: '<S880>/Action Port'

9087
           */

9088
          /* Sum: '<S888>/Add' incorporates:

9089
           *  Constant: '<S888>/Constant'

9090
           */

9091
          _rtB->Product_h = ((_rtB->Product2[0] - _rtB->Product2[4]) -

9092
                             _rtB->Product2[8]) + 1.0;

9093


9094
          /* Sqrt: '<S880>/sqrt' */

9095
          _rtB->Product_h = sqrt(_rtB->Product_h);

9096


9097
          /* Gain: '<S880>/Gain' */

9098
          _rtB->Merge[1] = 0.5 * _rtB->Product_h;

9099


9100
          /* Switch: '<S887>/Switch' incorporates:

9101
           *  Constant: '<S887>/Constant1'

9102
           */

9103
          if (_rtB->Product_h != 0.0) {

9104
            _rtB->Switch_gm[0] = 0.5;

9105
            _rtB->Switch_gm[1] = _rtB->Product_h;

9106
          } else {

9107
            _rtB->Switch_gm[0] = 0.0;

9108
            _rtB->Switch_gm[1] = 1.0;

9109
          }

9110


9111
          /* End of Switch: '<S887>/Switch' */

9112


9113
          /* Product: '<S887>/Product' */

9114
          _rtB->Product_h = _rtB->Switch_gm[0] / _rtB->Switch_gm[1];

9115


9116
          /* Sum: '<S886>/Add' */

9117
          _rtB->Add_d = _rtB->Product2[1] + _rtB->Product2[3];

9118


9119
          /* Sum: '<S884>/Add' */

9120
          _rtB->Add_es = _rtB->Product2[2] + _rtB->Product2[6];

9121


9122
          /* Sum: '<S885>/Add' */

9123
          _rtB->Add_gl = _rtB->Product2[7] - _rtB->Product2[5];

9124


9125
          /* Product: '<S880>/Product' */

9126
          _rtB->Product_p[0] = _rtB->Product_h * _rtB->Add_d;

9127
          _rtB->Product_p[1] = _rtB->Product_h * _rtB->Add_es;

9128
          _rtB->Product_p[2] = _rtB->Product_h * _rtB->Add_gl;

9129


9130
          /* Gain: '<S880>/Gain1' */

9131
          _rtB->Merge[2] = _rtB->Product_p[0];

9132


9133
          /* Gain: '<S880>/Gain2' */

9134
          _rtB->Merge[3] = _rtB->Product_p[1];

9135


9136
          /* Gain: '<S880>/Gain3' */

9137
          _rtB->Merge[0] = _rtB->Product_p[2];

9138


9139
          /* End of Outputs for SubSystem: '<S877>/Maximum Value at DCM(1,1)' */

9140
          break;

9141
        }

9142


9143
        /* End of If: '<S877>/Find Maximum Diagonal Value' */

9144
        /* End of Outputs for SubSystem: '<S847>/Negative Trace' */

9145
        break;

9146
      }

9147


9148
      /* End of If: '<S847>/If' */

9149


9150
      /* Integrator: '<S779>/q' */

9151
      if (((int_T *)ssGetDWork(S, 344))[0] != 0) {

9152
        _rtX->q_CSTATE[0] = _rtB->Merge[0];

9153
        _rtX->q_CSTATE[1] = _rtB->Merge[1];

9154
        _rtX->q_CSTATE[2] = _rtB->Merge[2];

9155
        _rtX->q_CSTATE[3] = _rtB->Merge[3];

9156
      }

9157


9158
      _rtB->q_j[0] = _rtX->q_CSTATE[0];

9159
      _rtB->q_j[1] = _rtX->q_CSTATE[1];

9160
      _rtB->q_j[2] = _rtX->q_CSTATE[2];

9161
      _rtB->q_j[3] = _rtX->q_CSTATE[3];

9162


9163
      /* End of Integrator: '<S779>/q' */

9164


9165
      /* Product: '<S939>/Product' */

9166
      _rtB->saturation = _rtB->q_j[0] * _rtB->q_j[0];

9167


9168
      /* Product: '<S939>/Product1' */

9169
      _rtB->table2D_Cm_dr = _rtB->q_j[1] * _rtB->q_j[1];

9170


9171
      /* Product: '<S939>/Product2' */

9172
      _rtB->UnitConversion_j = _rtB->q_j[2] * _rtB->q_j[2];

9173


9174
      /* Product: '<S939>/Product3' */

9175
      _rtB->add_d = _rtB->q_j[3] * _rtB->q_j[3];

9176


9177
      /* Sum: '<S939>/Sum' */

9178
      _rtB->saturation = ((_rtB->saturation + _rtB->table2D_Cm_dr) +

9179
                          _rtB->UnitConversion_j) + _rtB->add_d;

9180


9181
      /* Sqrt: '<S938>/sqrt' */

9182
      _rtB->saturation = sqrt(_rtB->saturation);

9183


9184
      /* Product: '<S937>/Product' */

9185
      _rtB->add_d = _rtB->q_j[0] / _rtB->saturation;

9186


9187
      /* Product: '<S927>/Product3' */

9188
      _rtB->UnitConversion_j = _rtB->add_d * _rtB->add_d;

9189


9190
      /* Product: '<S937>/Product1' */

9191
      _rtB->table2D_Cm_dr = _rtB->q_j[1] / _rtB->saturation;

9192


9193
      /* Product: '<S927>/Product2' */

9194
      _rtB->table2D_Cn = _rtB->table2D_Cm_dr * _rtB->table2D_Cm_dr;

9195


9196
      /* Product: '<S937>/Product2' */

9197
      _rtB->Product4_n = _rtB->q_j[2] / _rtB->saturation;

9198


9199
      /* Product: '<S927>/Product1' */

9200
      _rtB->u2rhoV2 = _rtB->Product4_n * _rtB->Product4_n;

9201


9202
      /* Product: '<S937>/Product3' */

9203
      _rtB->saturation = _rtB->q_j[3] / _rtB->saturation;

9204


9205
      /* Product: '<S927>/Product' */

9206
      _rtB->Sum3_j = _rtB->saturation * _rtB->saturation;

9207


9208
      /* Sum: '<S927>/Sum' */

9209
      _rtB->VectorConcatenate_n[0] = ((_rtB->UnitConversion_j + _rtB->table2D_Cn)

9210
        - _rtB->u2rhoV2) - _rtB->Sum3_j;

9211


9212
      /* Product: '<S930>/Product3' */

9213
      _rtB->Sum3_j = _rtB->saturation * _rtB->add_d;

9214


9215
      /* Product: '<S930>/Product2' */

9216
      _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->Product4_n;

9217


9218
      /* Sum: '<S930>/Sum' */

9219
      _rtB->Sum3_j = _rtB->u2rhoV2 - _rtB->Sum3_j;

9220


9221
      /* Gain: '<S930>/Gain' */

9222
      _rtB->VectorConcatenate_n[1] = 2.0 * _rtB->Sum3_j;

9223


9224
      /* Product: '<S933>/Product1' */

9225
      _rtB->Sum3_j = _rtB->add_d * _rtB->Product4_n;

9226


9227
      /* Product: '<S933>/Product2' */

9228
      _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->saturation;

9229


9230
      /* Sum: '<S933>/Sum' */

9231
      _rtB->Sum3_j += _rtB->u2rhoV2;

9232


9233
      /* Gain: '<S933>/Gain' */

9234
      _rtB->VectorConcatenate_n[2] = 2.0 * _rtB->Sum3_j;

9235


9236
      /* Product: '<S928>/Product3' */

9237
      _rtB->Sum3_j = _rtB->saturation * _rtB->add_d;

9238


9239
      /* Product: '<S928>/Product2' */

9240
      _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->Product4_n;

9241


9242
      /* Sum: '<S928>/Sum' */

9243
      _rtB->Sum3_j += _rtB->u2rhoV2;

9244


9245
      /* Gain: '<S928>/Gain' */

9246
      _rtB->VectorConcatenate_n[3] = 2.0 * _rtB->Sum3_j;

9247


9248
      /* Product: '<S931>/Product3' */

9249
      _rtB->Sum3_j = _rtB->add_d * _rtB->add_d;

9250


9251
      /* Product: '<S931>/Product2' */

9252
      _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->table2D_Cm_dr;

9253


9254
      /* Product: '<S931>/Product1' */

9255
      _rtB->table2D_Cn = _rtB->Product4_n * _rtB->Product4_n;

9256


9257
      /* Product: '<S931>/Product' */

9258
      _rtB->UnitConversion_j = _rtB->saturation * _rtB->saturation;

9259


9260
      /* Sum: '<S931>/Sum' */

9261
      _rtB->VectorConcatenate_n[4] = ((_rtB->Sum3_j - _rtB->u2rhoV2) +

9262
        _rtB->table2D_Cn) - _rtB->UnitConversion_j;

9263


9264
      /* Product: '<S934>/Product1' */

9265
      _rtB->Sum3_j = _rtB->add_d * _rtB->table2D_Cm_dr;

9266


9267
      /* Product: '<S934>/Product2' */

9268
      _rtB->u2rhoV2 = _rtB->Product4_n * _rtB->saturation;

9269


9270
      /* Sum: '<S934>/Sum' */

9271
      _rtB->Sum3_j = _rtB->u2rhoV2 - _rtB->Sum3_j;

9272


9273
      /* Gain: '<S934>/Gain' */

9274
      _rtB->VectorConcatenate_n[5] = 2.0 * _rtB->Sum3_j;

9275


9276
      /* Product: '<S929>/Product1' */

9277
      _rtB->Sum3_j = _rtB->add_d * _rtB->Product4_n;

9278


9279
      /* Product: '<S929>/Product2' */

9280
      _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->saturation;

9281


9282
      /* Sum: '<S929>/Sum' */

9283
      _rtB->Sum3_j = _rtB->u2rhoV2 - _rtB->Sum3_j;

9284


9285
      /* Gain: '<S929>/Gain' */

9286
      _rtB->VectorConcatenate_n[6] = 2.0 * _rtB->Sum3_j;

9287


9288
      /* Product: '<S932>/Product1' */

9289
      _rtB->Sum3_j = _rtB->add_d * _rtB->table2D_Cm_dr;

9290


9291
      /* Product: '<S932>/Product2' */

9292
      _rtB->u2rhoV2 = _rtB->Product4_n * _rtB->saturation;

9293


9294
      /* Sum: '<S932>/Sum' */

9295
      _rtB->Sum3_j += _rtB->u2rhoV2;

9296


9297
      /* Gain: '<S932>/Gain' */

9298
      _rtB->VectorConcatenate_n[7] = 2.0 * _rtB->Sum3_j;

9299


9300
      /* Product: '<S935>/Product3' */

9301
      _rtB->add_d *= _rtB->add_d;

9302


9303
      /* Product: '<S935>/Product2' */

9304
      _rtB->table2D_Cm_dr *= _rtB->table2D_Cm_dr;

9305


9306
      /* Product: '<S935>/Product1' */

9307
      _rtB->Product4_n *= _rtB->Product4_n;

9308


9309
      /* Product: '<S935>/Product' */

9310
      _rtB->saturation *= _rtB->saturation;

9311


9312
      /* Sum: '<S935>/Sum' */

9313
      _rtB->VectorConcatenate_n[8] = ((_rtB->add_d - _rtB->table2D_Cm_dr) -

9314
        _rtB->Product4_n) + _rtB->saturation;

9315
    }

9316


9317
    if (ssIsSampleHit(S, 1, 0)) {

9318
      /* Abs: '<S838>/Abs' incorporates:

9319
       *  Constant: '<S14>/Constant2'

9320
       */

9321
      _rtB->Switch_ly = 40.0;

9322


9323
      /* RelationalOperator: '<S839>/Compare' incorporates:

9324
       *  Constant: '<S839>/Constant'

9325
       */

9326
      _rtB->Compare_e = (_rtB->Switch_ly > 180.0);

9327


9328
      /* Switch: '<S838>/Switch' incorporates:

9329
       *  Constant: '<S14>/Constant2'

9330
       */

9331
      if (_rtB->Compare_e) {

9332
        /* Bias: '<S838>/Bias' incorporates:

9333
         *  Constant: '<S14>/Constant2'

9334
         */

9335
        _rtB->MathFunction1_n = 220.0;

9336


9337
        /* Math: '<S838>/Math Function1' incorporates:

9338
         *  Constant: '<S838>/Constant2'

9339
         */

9340
        _rtB->MathFunction1_n = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_n,

9341
          360.0);

9342


9343
        /* Bias: '<S838>/Bias1' */

9344
        _rtB->Bias1_i = _rtB->MathFunction1_n + -180.0;

9345
        _rtB->Switch_b = _rtB->Bias1_i;

9346
      } else {

9347
        _rtB->Switch_b = 40.0;

9348
      }

9349


9350
      /* End of Switch: '<S838>/Switch' */

9351


9352
      /* Abs: '<S835>/Abs1' */

9353
      _rtB->Abs1_e = fabs(_rtB->Switch_b);

9354


9355
      /* RelationalOperator: '<S837>/Compare' incorporates:

9356
       *  Constant: '<S837>/Constant'

9357
       */

9358
      _rtB->Compare_e = (_rtB->Abs1_e > 90.0);

9359


9360
      /* Switch: '<S835>/Switch' */

9361
      if (_rtB->Compare_e) {

9362
        /* Bias: '<S835>/Bias' */

9363
        _rtB->Bias1_ac = _rtB->Abs1_e + -90.0;

9364


9365
        /* Gain: '<S835>/Gain' */

9366
        _rtB->Bias1_ac = -_rtB->Bias1_ac;

9367


9368
        /* Bias: '<S835>/Bias1' */

9369
        _rtB->Bias1_ac += 90.0;

9370


9371
        /* Signum: '<S835>/Sign1' */

9372
        if (_rtB->Switch_b < 0.0) {

9373
          _rtB->Sign1 = -1.0;

9374
        } else if (_rtB->Switch_b > 0.0) {

9375
          _rtB->Sign1 = 1.0;

9376
        } else if (_rtB->Switch_b == 0.0) {

9377
          _rtB->Sign1 = 0.0;

9378
        } else {

9379
          _rtB->Sign1 = _rtB->Switch_b;

9380
        }

9381


9382
        /* End of Signum: '<S835>/Sign1' */

9383


9384
        /* Product: '<S835>/Divide1' */

9385
        _rtB->Divide1 = _rtB->Sign1 * _rtB->Bias1_ac;

9386
        _rtB->Switch_ly = _rtB->Divide1;

9387
      } else {

9388
        _rtB->Switch_ly = _rtB->Switch_b;

9389
      }

9390


9391
      /* End of Switch: '<S835>/Switch' */

9392


9393
      /* Gain: '<S829>/Unit Conversion' */

9394
      _rtB->u2u3 = 0.017453292519943295 * _rtB->Switch_ly;

9395


9396
      /* Trigonometry: '<S804>/sincos1' */

9397
      _rtB->sqrt_b = sin(_rtB->u2u3);

9398
      _rtB->Product2_fj = cos(_rtB->u2u3);

9399


9400
      /* Product: '<S804>/Product3' incorporates:

9401
       *  Constant: '<S800>/Constant2'

9402
       */

9403
      _rtB->sincos_o1_h[0] = 0.0 * _rtB->sqrt_b;

9404
      _rtB->sincos_o1_h[1] = 0.0 * _rtB->Product2_fj;

9405


9406
      /* Trigonometry: '<S804>/Trigonometric Function1' */

9407
      _rtB->u2u3 = tan(_rtB->u2u3);

9408


9409
      /* Product: '<S804>/Product2' */

9410
      _rtB->u2u3 = _rtB->u2u3 * AFCS_MODEL1_rtC(S)->Sum_bq * AFCS_MODEL1_rtC(S

9411
        )->Sum_bq;

9412


9413
      /* Trigonometry: '<S804>/Trigonometric Function2' incorporates:

9414
       *  Constant: '<S804>/f'

9415
       */

9416
      _rtB->u2u3 = AFCS_MODEL1_rt_atan2d_snf(_rtB->u2u3, 1.0);

9417


9418
      /* Gain: '<S831>/Unit Conversion' */

9419
      _rtB->Product2_fj = 57.295779513082323 * _rtB->u2u3;

9420


9421
      /* Gain: '<S834>/Unit Conversion' */

9422
      _rtB->Product2_fj *= 0.017453292519943295;

9423


9424
      /* Trigonometry: '<S833>/Trigonometric Function' */

9425
      _rtB->Product2_fj = sin(_rtB->Product2_fj);

9426


9427
      /* Product: '<S833>/Product2' */

9428
      _rtB->Product2_fj = _rtB->Product2_fj * _rtB->Product2_fj *

9429
        AFCS_MODEL1_rtC(S)->Sum1_e;

9430


9431
      /* Sum: '<S833>/Sum2' incorporates:

9432
       *  Constant: '<S833>/Constant'

9433
       */

9434
      _rtB->Product2_fj++;

9435


9436
      /* Product: '<S833>/Product3' incorporates:

9437
       *  Constant: '<S833>/Re'

9438
       */

9439
      _rtB->Product2_fj = 4.0589641E+13 / _rtB->Product2_fj;

9440


9441
      /* Sqrt: '<S833>/sqrt' */

9442
      _rtB->Product2_fj = sqrt(_rtB->Product2_fj);

9443


9444
      /* Trigonometry: '<S804>/sincos' */

9445
      _rtB->sqrt_b = cos(_rtB->u2u3);

9446
      _rtB->u2u3 = sin(_rtB->u2u3);

9447


9448
      /* Product: '<S804>/Product1' */

9449
      _rtB->sincos_o2_k[0] = _rtB->Product2_fj * _rtB->u2u3;

9450
      _rtB->sincos_o2_k[1] = _rtB->Product2_fj * _rtB->sqrt_b;

9451


9452
      /* Sum: '<S804>/Sum1' */

9453
      _rtB->sincos_o1_h[0] += _rtB->sincos_o2_k[0];

9454
      _rtB->sincos_o1_h[1] += _rtB->sincos_o2_k[1];

9455


9456
      /* Trigonometry: '<S804>/Trigonometric Function3' */

9457
      _rtB->Product2_fj = AFCS_MODEL1_rt_atan2d_snf(_rtB->sincos_o1_h[0],

9458
        _rtB->sincos_o1_h[1]);

9459


9460
      /* Gain: '<S830>/Unit Conversion' */

9461
      _rtB->Product2_fj *= 57.295779513082323;

9462


9463
      /* Switch: '<S805>/Switch1' incorporates:

9464
       *  Constant: '<S805>/Constant'

9465
       *  Constant: '<S805>/Constant1'

9466
       */

9467
      if (_rtB->Compare_e) {

9468
        _rtB->sqrt_b = 180.0;

9469
      } else {

9470
        _rtB->sqrt_b = 0.0;

9471
      }

9472


9473
      /* End of Switch: '<S805>/Switch1' */

9474


9475
      /* Sum: '<S805>/Sum' incorporates:

9476
       *  Constant: '<S14>/Constant1'

9477
       */

9478
      _rtB->Sum_i = _rtB->sqrt_b + 30.0;

9479


9480
      /* Abs: '<S836>/Abs' */

9481
      _rtB->sqrt_b = fabs(_rtB->Sum_i);

9482


9483
      /* RelationalOperator: '<S840>/Compare' incorporates:

9484
       *  Constant: '<S840>/Constant'

9485
       */

9486
      _rtB->Compare_e = (_rtB->sqrt_b > 180.0);

9487


9488
      /* Switch: '<S836>/Switch' */

9489
      if (_rtB->Compare_e) {

9490
        /* Bias: '<S836>/Bias' */

9491
        _rtB->MathFunction1 = _rtB->Sum_i + 180.0;

9492


9493
        /* Math: '<S836>/Math Function1' incorporates:

9494
         *  Constant: '<S836>/Constant2'

9495
         */

9496
        _rtB->MathFunction1 = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1, 360.0);

9497


9498
        /* Bias: '<S836>/Bias1' */

9499
        _rtB->Bias1 = _rtB->MathFunction1 + -180.0;

9500
        _rtB->sqrt_b = _rtB->Bias1;

9501
      } else {

9502
        _rtB->sqrt_b = _rtB->Sum_i;

9503
      }

9504


9505
      /* End of Switch: '<S836>/Switch' */

9506


9507
      /* Gain: '<S827>/Unit Conversion' */

9508
      _rtB->sincos_o1_h[0] = 0.017453292519943295 * _rtB->Product2_fj;

9509
      _rtB->sincos_o1_h[1] = 0.017453292519943295 * _rtB->sqrt_b;

9510


9511
      /* Trigonometry: '<S803>/sincos' */

9512
      TrigonometricFunction4_o = cos(_rtB->sincos_o1_h[0]);

9513
      _rtB->sincos_o1_h[0] = sin(_rtB->sincos_o1_h[0]);

9514
      _rtB->sincos_o2_k[0] = TrigonometricFunction4_o;

9515
      TrigonometricFunction4_o = cos(_rtB->sincos_o1_h[1]);

9516
      _rtB->sincos_o1_h[1] = sin(_rtB->sincos_o1_h[1]);

9517
      _rtB->sincos_o2_k[1] = TrigonometricFunction4_o;

9518


9519
      /* Product: '<S818>/u(1)*u(4)' */

9520
      _rtB->u2u3 = _rtB->sincos_o1_h[0] * _rtB->sincos_o2_k[1];

9521


9522
      /* UnaryMinus: '<S818>/Unary Minus' */

9523
      _rtB->VectorConcatenate_k[0] = -_rtB->u2u3;

9524


9525
      /* UnaryMinus: '<S821>/Unary Minus' */

9526
      _rtB->VectorConcatenate_k[1] = -_rtB->sincos_o1_h[1];

9527


9528
      /* Product: '<S824>/u(3)*u(4)' */

9529
      _rtB->u2u3 = _rtB->sincos_o2_k[0] * _rtB->sincos_o2_k[1];

9530


9531
      /* UnaryMinus: '<S824>/Unary Minus' */

9532
      _rtB->VectorConcatenate_k[2] = -_rtB->u2u3;

9533


9534
      /* Product: '<S819>/u(1)*u(2)' */

9535
      _rtB->u2u3 = _rtB->sincos_o1_h[0] * _rtB->sincos_o1_h[1];

9536


9537
      /* UnaryMinus: '<S819>/Unary Minus' */

9538
      _rtB->VectorConcatenate_k[3] = -_rtB->u2u3;

9539


9540
      /* SignalConversion: '<S828>/ConcatBufferAtVector ConcatenateIn5' */

9541
      _rtB->VectorConcatenate_k[4] = _rtB->sincos_o2_k[1];

9542


9543
      /* Product: '<S825>/u(2)*u(3)' */

9544
      _rtB->u2u3 = _rtB->sincos_o1_h[1] * _rtB->sincos_o2_k[0];

9545


9546
      /* UnaryMinus: '<S825>/Unary Minus' */

9547
      _rtB->VectorConcatenate_k[5] = -_rtB->u2u3;

9548


9549
      /* SignalConversion: '<S828>/ConcatBufferAtVector ConcatenateIn7' */

9550
      _rtB->VectorConcatenate_k[6] = _rtB->sincos_o2_k[0];

9551


9552
      /* SignalConversion: '<S828>/ConcatBufferAtVector ConcatenateIn8' incorporates:

9553
       *  Constant: '<S823>/Constant'

9554
       */

9555
      _rtB->VectorConcatenate_k[7] = 0.0;

9556


9557
      /* UnaryMinus: '<S826>/Unary Minus' */

9558
      _rtB->VectorConcatenate_k[8] = -_rtB->sincos_o1_h[0];

9559


9560
      /* Math: '<S800>/Math Function1' */

9561
      for (i_1 = 0; i_1 < 3; i_1++) {

9562
        _rtB->MathFunction[3 * i_1] = _rtB->VectorConcatenate_k[i_1];

9563
        _rtB->MathFunction[1 + 3 * i_1] = _rtB->VectorConcatenate_k[i_1 + 3];

9564
        _rtB->MathFunction[2 + 3 * i_1] = _rtB->VectorConcatenate_k[i_1 + 6];

9565
      }

9566


9567
      /* End of Math: '<S800>/Math Function1' */

9568


9569
      /* Gain: '<S841>/Unit Conversion' */

9570
      _rtB->Product2_fj *= 0.017453292519943295;

9571


9572
      /* Trigonometry: '<S806>/Trigonometric Function' */

9573
      _rtB->Product2_fj = sin(_rtB->Product2_fj);

9574


9575
      /* Product: '<S806>/Product2' */

9576
      _rtB->Product2_fj = _rtB->Product2_fj * _rtB->Product2_fj *

9577
        AFCS_MODEL1_rtC(S)->Sum1_b;

9578


9579
      /* Sum: '<S806>/Sum2' incorporates:

9580
       *  Constant: '<S806>/Constant'

9581
       */

9582
      _rtB->Product2_fj++;

9583


9584
      /* Product: '<S806>/Product3' incorporates:

9585
       *  Constant: '<S806>/Re'

9586
       */

9587
      _rtB->Product2_fj = 4.0589641E+13 / _rtB->Product2_fj;

9588


9589
      /* Sqrt: '<S806>/sqrt' */

9590
      _rtB->Product2_fj = sqrt(_rtB->Product2_fj);

9591


9592
      /* UnaryMinus: '<S800>/Unary Minus' */

9593
      _rtB->Product2_fj = -_rtB->Product2_fj;

9594


9595
      /* SignalConversion: '<S800>/TmpSignal ConversionAtProduct1Inport2' incorporates:

9596
       *  Constant: '<S800>/Constant'

9597
       */

9598
      _rtB->TmpSignalConversionAtProductInp[0] = 0.0;

9599
      _rtB->TmpSignalConversionAtProductInp[1] = 0.0;

9600
      _rtB->TmpSignalConversionAtProductInp[2] = _rtB->Product2_fj;

9601


9602
      /* Product: '<S800>/Product1' */

9603
      for (i_1 = 0; i_1 < 3; i_1++) {

9604
        _rtB->Product1_ec[i_1] = 0.0;

9605
        _rtB->Product1_ec[i_1] += _rtB->MathFunction[i_1] *

9606
          _rtB->TmpSignalConversionAtProductInp[0];

9607
        _rtB->Product1_ec[i_1] += _rtB->MathFunction[i_1 + 3] *

9608
          _rtB->TmpSignalConversionAtProductInp[1];

9609
        _rtB->Product1_ec[i_1] += _rtB->MathFunction[i_1 + 6] *

9610
          _rtB->TmpSignalConversionAtProductInp[2];

9611
      }

9612


9613
      /* End of Product: '<S800>/Product1' */

9614


9615
      /* Gain: '<S816>/Unit Conversion' */

9616
      _rtB->sincos_o1_h[0] = 0.017453292519943295 * _rtB->Switch_ly;

9617
      _rtB->sincos_o1_h[1] = 0.017453292519943295 * _rtB->sqrt_b;

9618


9619
      /* Trigonometry: '<S802>/sincos' */

9620
      TrigonometricFunction4_o = cos(_rtB->sincos_o1_h[0]);

9621
      _rtB->sincos_o1_h[0] = sin(_rtB->sincos_o1_h[0]);

9622
      _rtB->sincos_o2_k[0] = TrigonometricFunction4_o;

9623
      TrigonometricFunction4_o = cos(_rtB->sincos_o1_h[1]);

9624
      _rtB->sincos_o1_h[1] = sin(_rtB->sincos_o1_h[1]);

9625
      _rtB->sincos_o2_k[1] = TrigonometricFunction4_o;

9626


9627
      /* Product: '<S807>/u(1)*u(4)' */

9628
      _rtB->Product2_fj = _rtB->sincos_o1_h[0] * _rtB->sincos_o2_k[1];

9629


9630
      /* UnaryMinus: '<S807>/Unary Minus' */

9631
      _rtB->VectorConcatenate_be[0] = -_rtB->Product2_fj;

9632


9633
      /* UnaryMinus: '<S810>/Unary Minus' */

9634
      _rtB->VectorConcatenate_be[1] = -_rtB->sincos_o1_h[1];

9635


9636
      /* Product: '<S813>/u(3)*u(4)' */

9637
      _rtB->Product2_fj = _rtB->sincos_o2_k[0] * _rtB->sincos_o2_k[1];

9638


9639
      /* UnaryMinus: '<S813>/Unary Minus' */

9640
      _rtB->VectorConcatenate_be[2] = -_rtB->Product2_fj;

9641


9642
      /* Product: '<S808>/u(1)*u(2)' */

9643
      _rtB->Product2_fj = _rtB->sincos_o1_h[0] * _rtB->sincos_o1_h[1];

9644


9645
      /* UnaryMinus: '<S808>/Unary Minus' */

9646
      _rtB->VectorConcatenate_be[3] = -_rtB->Product2_fj;

9647


9648
      /* SignalConversion: '<S817>/ConcatBufferAtVector ConcatenateIn5' */

9649
      _rtB->VectorConcatenate_be[4] = _rtB->sincos_o2_k[1];

9650


9651
      /* Product: '<S814>/u(2)*u(3)' */

9652
      _rtB->Product2_fj = _rtB->sincos_o1_h[1] * _rtB->sincos_o2_k[0];

9653


9654
      /* UnaryMinus: '<S814>/Unary Minus' */

9655
      _rtB->VectorConcatenate_be[5] = -_rtB->Product2_fj;

9656


9657
      /* SignalConversion: '<S817>/ConcatBufferAtVector ConcatenateIn7' */

9658
      _rtB->VectorConcatenate_be[6] = _rtB->sincos_o2_k[0];

9659


9660
      /* SignalConversion: '<S817>/ConcatBufferAtVector ConcatenateIn8' incorporates:

9661
       *  Constant: '<S812>/Constant'

9662
       */

9663
      _rtB->VectorConcatenate_be[7] = 0.0;

9664


9665
      /* UnaryMinus: '<S815>/Unary Minus' */

9666
      _rtB->VectorConcatenate_be[8] = -_rtB->sincos_o1_h[0];

9667


9668
      /* Math: '<S800>/Math Function' */

9669
      for (i_1 = 0; i_1 < 3; i_1++) {

9670
        _rtB->MathFunction[3 * i_1] = _rtB->VectorConcatenate_be[i_1];

9671
        _rtB->MathFunction[1 + 3 * i_1] = _rtB->VectorConcatenate_be[i_1 + 3];

9672
        _rtB->MathFunction[2 + 3 * i_1] = _rtB->VectorConcatenate_be[i_1 + 6];

9673
      }

9674


9675
      /* End of Math: '<S800>/Math Function' */

9676


9677
      /* UnaryMinus: '<S800>/Unary Minus1' */

9678
      _rtB->Product2_fj = -_rtB->Gain11;

9679


9680
      /* SignalConversion: '<S800>/TmpSignal ConversionAtProductInport2' incorporates:

9681
       *  Constant: '<S800>/Constant1'

9682
       */

9683
      _rtB->TmpSignalConversionAtProductInp[0] = 0.0;

9684
      _rtB->TmpSignalConversionAtProductInp[1] = 0.0;

9685
      _rtB->TmpSignalConversionAtProductInp[2] = _rtB->Product2_fj;

9686
      for (i_1 = 0; i_1 < 3; i_1++) {

9687
        /* Product: '<S800>/Product' */

9688
        _rtB->Product_g5[i_1] = 0.0;

9689
        _rtB->Product_g5[i_1] += _rtB->MathFunction[i_1] *

9690
          _rtB->TmpSignalConversionAtProductInp[0];

9691
        _rtB->Product_g5[i_1] += _rtB->MathFunction[i_1 + 3] *

9692
          _rtB->TmpSignalConversionAtProductInp[1];

9693
        _rtB->Product_g5[i_1] += _rtB->MathFunction[i_1 + 6] *

9694
          _rtB->TmpSignalConversionAtProductInp[2];

9695


9696
        /* Sum: '<S800>/Sum' */

9697
        _rtB->Sum_k[i_1] = _rtB->Product1_ec[i_1] + _rtB->Product_g5[i_1];

9698
      }

9699
    }

9700


9701
    if (1) {

9702
      /* Integrator: '<S778>/p1' */

9703
      if (((int_T *)ssGetDWork(S, 345))[0] != 0) {

9704
        _rtX->p1_CSTATE[0] = _rtB->Sum_k[0];

9705
        _rtX->p1_CSTATE[1] = _rtB->Sum_k[1];

9706
        _rtX->p1_CSTATE[2] = _rtB->Sum_k[2];

9707
      }

9708


9709
      _rtB->p1[0] = _rtX->p1_CSTATE[0];

9710
      _rtB->p1[1] = _rtX->p1_CSTATE[1];

9711
      _rtB->p1[2] = _rtX->p1_CSTATE[2];

9712


9713
      /* End of Integrator: '<S778>/p1' */

9714


9715
      /* Product: '<S785>/Product2' */

9716
      _rtB->Sum3_j = _rtB->p1[0] * _rtB->p1[0];

9717


9718
      /* Product: '<S785>/Product3' */

9719
      _rtB->u2rhoV2 = _rtB->p1[1] * _rtB->p1[1];

9720


9721
      /* Sum: '<S785>/Sum2' */

9722
      _rtB->Sum3_j += _rtB->u2rhoV2;

9723


9724
      /* Sqrt: '<S785>/sqrt' */

9725
      _rtB->sqrt_m = sqrt(_rtB->Sum3_j);

9726
    }

9727


9728
    if (ssIsSampleHit(S, 1, 0)) {

9729
      /* Outputs for Iterator SubSystem: '<S774>/While Iterator Subsystem' incorporates:

9730
       *  WhileIterator: '<S787>/While Iterator'

9731
       */

9732
      rtAction = 1;

9733


9734
      /* End of Outputs for SubSystem: '<S774>/While Iterator Subsystem' */

9735
      do {

9736
        _rtB->WhileIterator = rtAction;

9737
        _rtB->Switch_na = ((real_T *)ssGetDWork(S, 11))[0];

9738
        if (!(_rtB->WhileIterator > 0)) {

9739
          _rtB->Product2_in = AFCS_MODEL1_rtC(S)->Sum1_k * _rtB->sqrt_m;

9740
          _rtB->TrigonometricFunction3 = AFCS_MODEL1_rt_atan2d_snf(_rtB->p1[2],

9741
            _rtB->Product2_in);

9742
          _rtB->Switch_na = _rtB->TrigonometricFunction3;

9743
        }

9744


9745
        _rtB->sincos_o2_c = sin(_rtB->Switch_na);

9746
        _rtB->Product3_e = cos(_rtB->Switch_na);

9747
        _rtB->sincos_o2_c *= AFCS_MODEL1_rtC(S)->Product2_o * AFCS_MODEL1_rtC(S

9748
          )->Product1_l5 * _rtB->sincos_o2_c * _rtB->sincos_o2_c;

9749
        _rtB->sincos_o2_c += _rtB->p1[2];

9750
        _rtB->Product3_e = _rtB->Product3_e * _rtB->Product3_e *

9751
          _rtB->Product3_e * AFCS_MODEL1_rtC(S)->Sum1_o * 6.371E+6;

9752
        _rtB->Product3_e = _rtB->sqrt_m - _rtB->Product3_e;

9753
        _rtB->TrigonometricFunction4 = AFCS_MODEL1_rt_atan2d_snf

9754
          (_rtB->sincos_o2_c, _rtB->Product3_e);

9755
        _rtB->Product3_e = sin(_rtB->TrigonometricFunction4);

9756
        _rtB->sincos_o2_c = cos(_rtB->TrigonometricFunction4);

9757
        _rtB->Product3_e *= AFCS_MODEL1_rtC(S)->Sum1_k;

9758
        TrigonometricFunction4_o = AFCS_MODEL1_rt_atan2d_snf(_rtB->Product3_e,

9759
          _rtB->sincos_o2_c);

9760
        _rtB->RelationalOperator = (_rtB->Switch_na != TrigonometricFunction4_o);

9761
        ((real_T *)ssGetDWork(S, 11))[0] = TrigonometricFunction4_o;

9762
        rtAction++;

9763
      } while (_rtB->RelationalOperator && (rtAction <= 5));

9764
    }

9765


9766
    if (1) {

9767
      /* Trigonometry: '<S774>/Trigonometric Function2' */

9768
      _rtB->Sum3_j = AFCS_MODEL1_rt_atan2d_snf(_rtB->p1[1], _rtB->p1[0]);

9769


9770
      /* Gain: '<S784>/Unit Conversion' */

9771
      _rtB->UnitConversion_c4[0] = 57.295779513082323 *

9772
        _rtB->TrigonometricFunction4;

9773
      _rtB->UnitConversion_c4[1] = 57.295779513082323 * _rtB->Sum3_j;

9774


9775
      /* Gain: '<S861>/Unit Conversion' */

9776
      _rtB->sincos_o2_h[0] = 0.017453292519943295 * _rtB->UnitConversion_c4[0];

9777


9778
      /* Trigonometry: '<S844>/sincos' */

9779
      TrigonometricFunction4_o = cos(_rtB->sincos_o2_h[0]);

9780
      _rtB->sincos_o2_h[0] = sin(_rtB->sincos_o2_h[0]);

9781
      _rtB->sincos_o1_m[0] = TrigonometricFunction4_o;

9782


9783
      /* Gain: '<S861>/Unit Conversion' */

9784
      _rtB->sincos_o2_h[1] = 0.017453292519943295 * _rtB->UnitConversion_c4[1];

9785


9786
      /* Trigonometry: '<S844>/sincos' */

9787
      TrigonometricFunction4_o = cos(_rtB->sincos_o2_h[1]);

9788
      _rtB->sincos_o2_h[1] = sin(_rtB->sincos_o2_h[1]);

9789
      _rtB->sincos_o1_m[1] = TrigonometricFunction4_o;

9790


9791
      /* Product: '<S852>/u(1)*u(4)' */

9792
      _rtB->Sum3_j = _rtB->sincos_o2_h[0] * _rtB->sincos_o1_m[1];

9793


9794
      /* UnaryMinus: '<S852>/Unary Minus' */

9795
      _rtB->VectorConcatenate_nn[0] = -_rtB->Sum3_j;

9796


9797
      /* UnaryMinus: '<S855>/Unary Minus' */

9798
      _rtB->VectorConcatenate_nn[1] = -_rtB->sincos_o2_h[1];

9799


9800
      /* Product: '<S858>/u(3)*u(4)' */

9801
      _rtB->Sum3_j = _rtB->sincos_o1_m[0] * _rtB->sincos_o1_m[1];

9802


9803
      /* UnaryMinus: '<S858>/Unary Minus' */

9804
      _rtB->VectorConcatenate_nn[2] = -_rtB->Sum3_j;

9805


9806
      /* Product: '<S853>/u(1)*u(2)' */

9807
      _rtB->Sum3_j = _rtB->sincos_o2_h[0] * _rtB->sincos_o2_h[1];

9808


9809
      /* UnaryMinus: '<S853>/Unary Minus' */

9810
      _rtB->VectorConcatenate_nn[3] = -_rtB->Sum3_j;

9811


9812
      /* SignalConversion: '<S862>/ConcatBufferAtVector ConcatenateIn5' */

9813
      _rtB->VectorConcatenate_nn[4] = _rtB->sincos_o1_m[1];

9814


9815
      /* Product: '<S859>/u(2)*u(3)' */

9816
      _rtB->Sum3_j = _rtB->sincos_o2_h[1] * _rtB->sincos_o1_m[0];

9817


9818
      /* UnaryMinus: '<S859>/Unary Minus' */

9819
      _rtB->VectorConcatenate_nn[5] = -_rtB->Sum3_j;

9820


9821
      /* SignalConversion: '<S862>/ConcatBufferAtVector ConcatenateIn7' */

9822
      _rtB->VectorConcatenate_nn[6] = _rtB->sincos_o1_m[0];

9823


9824
      /* SignalConversion: '<S862>/ConcatBufferAtVector ConcatenateIn8' incorporates:

9825
       *  Constant: '<S857>/Constant'

9826
       */

9827
      _rtB->VectorConcatenate_nn[7] = 0.0;

9828


9829
      /* UnaryMinus: '<S860>/Unary Minus' */

9830
      _rtB->VectorConcatenate_nn[8] = -_rtB->sincos_o2_h[0];

9831


9832
      /* Math: '<S779>/Math Function2' */

9833
      for (i_1 = 0; i_1 < 3; i_1++) {

9834
        _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_nn[i_1];

9835
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_nn[i_1 + 3];

9836
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_nn[i_1 + 6];

9837
      }

9838


9839
      /* End of Math: '<S779>/Math Function2' */

9840


9841
      /* Product: '<S779>/Product4' */

9842
      for (i_1 = 0; i_1 < 3; i_1++) {

9843
        for (flag = 0; flag < 3; flag++) {

9844
          _rtB->Product4[i_1 + 3 * flag] = 0.0;

9845
          _rtB->Product4[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag] *

9846
            _rtB->VectorConcatenate_n[i_1];

9847
          _rtB->Product4[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag + 1] *

9848
            _rtB->VectorConcatenate_n[i_1 + 3];

9849
          _rtB->Product4[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag + 2] *

9850
            _rtB->VectorConcatenate_n[i_1 + 6];

9851
        }

9852
      }

9853


9854
      /* End of Product: '<S779>/Product4' */

9855


9856
      /* SignalConversion: '<S615>/TmpSignal ConversionAtProduct2Inport2' incorporates:

9857
       *  Inport: '<Root>/Vx_wind'

9858
       *  Inport: '<Root>/Vy_wind'

9859
       *  Inport: '<Root>/Vz_wind'

9860
       */

9861
      _rtB->ubvbwb[0] = *((const real_T **)ssGetInputPortSignalPtrs(S, 5))[0];

9862
      _rtB->ubvbwb[1] = *((const real_T **)ssGetInputPortSignalPtrs(S, 6))[0];

9863
      _rtB->ubvbwb[2] = *((const real_T **)ssGetInputPortSignalPtrs(S, 7))[0];

9864


9865
      /* Product: '<S615>/Product2' */

9866
      for (i_1 = 0; i_1 < 3; i_1++) {

9867
        _rtB->Product2_i[i_1] = 0.0;

9868
        _rtB->Product2_i[i_1] += _rtB->Product4[i_1] * _rtB->ubvbwb[0];

9869
        _rtB->Product2_i[i_1] += _rtB->Product4[i_1 + 3] * _rtB->ubvbwb[1];

9870
        _rtB->Product2_i[i_1] += _rtB->Product4[i_1 + 6] * _rtB->ubvbwb[2];

9871
      }

9872


9873
      /* End of Product: '<S615>/Product2' */

9874
    }

9875


9876
    if (ssIsSampleHit(S, 1, 0)) {

9877
      /* Trigonometry: '<S786>/sincos' */

9878
      _rtB->sincos_o1 = sin(_rtB->TrigonometricFunction4);

9879
      _rtB->sincos_o2 = cos(_rtB->TrigonometricFunction4);

9880


9881
      /* Product: '<S786>/Product2' */

9882
      _rtB->Product2_fj = _rtB->sincos_o1 * AFCS_MODEL1_rtC(S)->Sum1_o;

9883


9884
      /* Product: '<S786>/Product1' */

9885
      _rtB->sqrt_b = _rtB->Product2_fj * _rtB->sincos_o1;

9886


9887
      /* Sum: '<S786>/Sum' incorporates:

9888
       *  Constant: '<S786>/f'

9889
       */

9890
      _rtB->sqrt_b = 1.0 - _rtB->sqrt_b;

9891


9892
      /* Sqrt: '<S786>/sqrt' */

9893
      _rtB->sqrt_b = sqrt(_rtB->sqrt_b);

9894


9895
      /* Product: '<S786>/Product3' incorporates:

9896
       *  Constant: '<S786>/Constant1'

9897
       */

9898
      _rtB->Product3 = 6.371E+6 / _rtB->sqrt_b;

9899


9900
      /* Product: '<S786>/Product4' */

9901
      _rtB->Product4_k = _rtB->Product2_fj * _rtB->Product3;

9902
    }

9903


9904
    if (1) {

9905
      /* Sum: '<S786>/Sum2' */

9906
      _rtB->Sum3_j = _rtB->p1[2] + _rtB->Product4_k;

9907


9908
      /* Product: '<S786>/Product5' */

9909
      _rtB->Sum3_j *= _rtB->sincos_o1;

9910


9911
      /* Product: '<S786>/Product6' */

9912
      _rtB->u2rhoV2 = _rtB->sincos_o2 * _rtB->sqrt_m;

9913


9914
      /* Sum: '<S786>/Sum3' */

9915
      _rtB->Sum3_j = (_rtB->Sum3_j - _rtB->Product3) + _rtB->u2rhoV2;

9916


9917
      /* Gain: '<S446>/Gain2' */

9918
      _rtB->Alt = _rtB->Sum3_j;

9919


9920
      /* Sum: '<S446>/Sum2' */

9921
      _rtB->u2rhoV2 = _rtB->Alt - _rtB->DataTypeConversion1;

9922


9923
      /* Switch: '<S465>/Switch1' incorporates:

9924
       *  Saturate: '<S465>/Saturation'

9925
       */

9926
      if (_rtB->u2rhoV2 > 30.0) {

9927
        tmpForInput_idx_0 = _rtB->Product2_i[0];

9928
        tmpForInput_idx_1 = _rtB->Product2_i[1];

9929
        tmpForInput_idx_2 = _rtB->Product2_i[2];

9930
      } else {

9931
        if (_rtB->Product2_i[0] > 10.0) {

9932
          /* Saturate: '<S465>/Saturation' */

9933
          _rtB->Saturation_b[0] = 10.0;

9934
        } else if (_rtB->Product2_i[0] < -10.0) {

9935
          /* Saturate: '<S465>/Saturation' */

9936
          _rtB->Saturation_b[0] = -10.0;

9937
        } else {

9938
          /* Saturate: '<S465>/Saturation' */

9939
          _rtB->Saturation_b[0] = _rtB->Product2_i[0];

9940
        }

9941


9942
        tmpForInput_idx_0 = _rtB->Saturation_b[0];

9943


9944
        /* Saturate: '<S465>/Saturation' */

9945
        if (_rtB->Product2_i[1] > 10.0) {

9946
          _rtB->Saturation_b[1] = 10.0;

9947
        } else if (_rtB->Product2_i[1] < -10.0) {

9948
          _rtB->Saturation_b[1] = -10.0;

9949
        } else {

9950
          _rtB->Saturation_b[1] = _rtB->Product2_i[1];

9951
        }

9952


9953
        tmpForInput_idx_1 = _rtB->Saturation_b[1];

9954


9955
        /* Saturate: '<S465>/Saturation' */

9956
        if (_rtB->Product2_i[2] > 10.0) {

9957
          _rtB->Saturation_b[2] = 10.0;

9958
        } else if (_rtB->Product2_i[2] < -10.0) {

9959
          _rtB->Saturation_b[2] = -10.0;

9960
        } else {

9961
          _rtB->Saturation_b[2] = _rtB->Product2_i[2];

9962
        }

9963


9964
        tmpForInput_idx_2 = _rtB->Saturation_b[2];

9965
      }

9966


9967
      /* End of Switch: '<S465>/Switch1' */

9968


9969
      /* RateLimiter: '<S465>/Rate Limiter' */

9970
      if (((real_T *)ssGetDWork(S, 3))[0] == (rtInf)) {

9971
        _rtB->RateLimiter[0] = tmpForInput_idx_0;

9972
        _rtB->RateLimiter[1] = tmpForInput_idx_1;

9973
        _rtB->RateLimiter[2] = tmpForInput_idx_2;

9974
      } else {

9975
        T = ssGetTaskTime(S,0) - ((real_T *)ssGetDWork(S, 3))[0];

9976
        TrigonometricFunction4_o = T * 2.0;

9977
        T *= -2.0;

9978
        rateLimiterRate = tmpForInput_idx_0 - ((real_T *)ssGetDWork(S, 2))[0];

9979
        if (rateLimiterRate > TrigonometricFunction4_o) {

9980
          _rtB->RateLimiter[0] = ((real_T *)ssGetDWork(S, 2))[0] +

9981
            TrigonometricFunction4_o;

9982
        } else if (rateLimiterRate < T) {

9983
          _rtB->RateLimiter[0] = ((real_T *)ssGetDWork(S, 2))[0] + T;

9984
        } else {

9985
          _rtB->RateLimiter[0] = tmpForInput_idx_0;

9986
        }

9987


9988
        rateLimiterRate = tmpForInput_idx_1 - ((real_T *)ssGetDWork(S, 2))[1];

9989
        if (rateLimiterRate > TrigonometricFunction4_o) {

9990
          _rtB->RateLimiter[1] = ((real_T *)ssGetDWork(S, 2))[1] +

9991
            TrigonometricFunction4_o;

9992
        } else if (rateLimiterRate < T) {

9993
          _rtB->RateLimiter[1] = ((real_T *)ssGetDWork(S, 2))[1] + T;

9994
        } else {

9995
          _rtB->RateLimiter[1] = tmpForInput_idx_1;

9996
        }

9997


9998
        rateLimiterRate = tmpForInput_idx_2 - ((real_T *)ssGetDWork(S, 2))[2];

9999
        if (rateLimiterRate > TrigonometricFunction4_o) {

10000
          _rtB->RateLimiter[2] = ((real_T *)ssGetDWork(S, 2))[2] +

10001
            TrigonometricFunction4_o;

10002
        } else if (rateLimiterRate < T) {

10003
          _rtB->RateLimiter[2] = ((real_T *)ssGetDWork(S, 2))[2] + T;

10004
        } else {

10005
          _rtB->RateLimiter[2] = tmpForInput_idx_2;

10006
        }

10007
      }

10008


10009
      /* End of RateLimiter: '<S465>/Rate Limiter' */

10010


10011
      /* Integrator: '<S780>/ub,vb,wb' */

10012
      if (((int_T *)ssGetDWork(S, 346))[0] != 0) {

10013
        _rtX->ubvbwb_CSTATE[0] = _rtB->uvw0[0];

10014
        _rtX->ubvbwb_CSTATE[1] = _rtB->uvw0[1];

10015
        _rtX->ubvbwb_CSTATE[2] = _rtB->uvw0[2];

10016
      }

10017


10018
      _rtB->ubvbwb[0] = _rtX->ubvbwb_CSTATE[0];

10019


10020
      /* Gain: '<S782>/Unit Conversion' */

10021
      _rtB->UnitConversion_o[0] = _rtB->ubvbwb[0];

10022


10023
      /* Sum: '<S465>/Sum' */

10024
      _rtB->Sum_m[0] = _rtB->UnitConversion_o[0] - _rtB->RateLimiter[0];

10025


10026
      /* Integrator: '<S780>/ub,vb,wb' */

10027
      _rtB->ubvbwb[1] = _rtX->ubvbwb_CSTATE[1];

10028


10029
      /* Gain: '<S782>/Unit Conversion' */

10030
      _rtB->UnitConversion_o[1] = _rtB->ubvbwb[1];

10031


10032
      /* Sum: '<S465>/Sum' */

10033
      _rtB->Sum_m[1] = _rtB->UnitConversion_o[1] - _rtB->RateLimiter[1];

10034


10035
      /* Integrator: '<S780>/ub,vb,wb' */

10036
      _rtB->ubvbwb[2] = _rtX->ubvbwb_CSTATE[2];

10037


10038
      /* Gain: '<S782>/Unit Conversion' */

10039
      _rtB->UnitConversion_o[2] = _rtB->ubvbwb[2];

10040


10041
      /* Sum: '<S465>/Sum' */

10042
      _rtB->Sum_m[2] = _rtB->UnitConversion_o[2] - _rtB->RateLimiter[2];

10043


10044
      /* Product: '<S609>/Product' */

10045
      _rtB->u2rhoV2 = _rtB->Sum_m[0] * _rtB->Sum_m[0];

10046


10047
      /* Product: '<S609>/Product1' */

10048
      _rtB->Product4_n = _rtB->Sum_m[1] * _rtB->Sum_m[1];

10049


10050
      /* Product: '<S609>/Product2' */

10051
      _rtB->table2D_Cn = _rtB->Sum_m[2] * _rtB->Sum_m[2];

10052


10053
      /* Sum: '<S609>/Sum' */

10054
      _rtB->u2rhoV2 = (_rtB->u2rhoV2 + _rtB->Product4_n) + _rtB->table2D_Cn;

10055


10056
      /* MATLAB Function: '<S464>/Embedded MATLAB Function1' */

10057
      /* MATLAB Function 'AFCS_MODEL1/Subsystem5/Subsystem3/Devond/Embedded MATLAB Function1': '<S604>:1' */

10058
      /* %%%%%%% */

10059
      /* '<S604>:1:3' */

10060
      /* '<S604>:1:4' */

10061
      TrigonometricFunction4_o = 6.371004E+6 * _rtB->Alt / (6.371004E+6 +

10062
        _rtB->Alt);

10063


10064
      /* %%%%%% */

10065
      if (_rtB->Alt <= 11019.0) {

10066
        /* '<S604>:1:6' */

10067
        /* '<S604>:1:7' */

10068
        T = 288.15 - 0.0065 * TrigonometricFunction4_o;

10069


10070
        /* '<S604>:1:8' */

10071
        _rtB->PS = AFCS_MODEL1_rt_powd_snf(1.0 - 2.2557700000000003E-5 *

10072
          TrigonometricFunction4_o, 5.25588) * 101325.0;

10073


10074
        /* '<S604>:1:9' */

10075
        _rtB->rou = AFCS_MODEL1_rt_powd_snf(1.0 - 2.2557700000000003E-5 *

10076
          TrigonometricFunction4_o, 4.25588) * 1.225;

10077
      } else if (_rtB->Alt <= 20063.0) {

10078
        /* '<S604>:1:10' */

10079
        /* '<S604>:1:11' */

10080
        T = 216.65;

10081


10082
        /* '<S604>:1:12' */

10083
        _rtB->PS = exp((TrigonometricFunction4_o - 11000.0) *

10084
                       -0.00015768850000000002) * 22632.04;

10085


10086
        /* '<S604>:1:13' */

10087
        _rtB->rou = exp((TrigonometricFunction4_o - 11000.0) *

10088
                        -0.00015768850000000002) * 0.3639176;

10089
      } else if (_rtB->Alt <= 32162.0) {

10090
        /* '<S604>:1:14' */

10091
        /* '<S604>:1:15' */

10092
        T = (TrigonometricFunction4_o - 20000.0) * 0.001 + 216.65;

10093


10094
        /* '<S604>:1:16' */

10095
        _rtB->PS = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 20000.0) *

10096
          4.6157399999999991E-6 + 1.0, -34.16322) * 5474.879;

10097


10098
        /* '<S604>:1:17' */

10099
        _rtB->rou = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 20000.0)

10100
          * 4.6157399999999991E-6 + 1.0, -35.16322) * 0.08803471;

10101
      } else if (_rtB->Alt <= 47350.0) {

10102
        /* '<S604>:1:18' */

10103
        /* '<S604>:1:19' */

10104
        T = (TrigonometricFunction4_o - 32000.0) * 0.0028 + 228.65;

10105


10106
        /* '<S604>:1:20' */

10107
        _rtB->PS = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 32000.0) *

10108
          1.224579E-5 + 1.0, -12.20115) * 868.01600000000008;

10109


10110
        /* '<S604>:1:21' */

10111
        _rtB->rou = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 32000.0)

10112
          * 1.224579E-5 + 1.0, -13.20115) * 0.01322497;

10113
      } else if (_rtB->Alt <= 51413.0) {

10114
        /* '<S604>:1:22' */

10115
        /* '<S604>:1:23' */

10116
        T = 270.65;

10117


10118
        /* '<S604>:1:24' */

10119
        _rtB->PS = exp((TrigonometricFunction4_o - 47000.0) *

10120
                       -0.00012622660000000002) * 110.90580000000001;

10121


10122
        /* '<S604>:1:25' */

10123
        _rtB->rou = exp((TrigonometricFunction4_o - 47000.0) *

10124
                        -0.00012622660000000002) * 0.001427527;

10125
      } else if (_rtB->Alt < 71802.0) {

10126
        /* '<S604>:1:26' */

10127
        /* '<S604>:1:27' */

10128
        T = 270.65 - (TrigonometricFunction4_o - 51000.0) * 0.0028;

10129


10130
        /* '<S604>:1:28' */

10131
        _rtB->PS = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -

10132
          51000.0) * 1.0345459999999998E-5, 12.20115) * 66.93853;

10133


10134
        /* '<S604>:1:29' */

10135
        _rtB->rou = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -

10136
          51000.0) * 1.0345459999999998E-5, 11.20115) * 0.00086160110000000007;

10137
      } else if (_rtB->Alt < 86000.0) {

10138
        /* '<S604>:1:30' */

10139
        /* '<S604>:1:31' */

10140
        T = 214.65 - (TrigonometricFunction4_o - 71000.0) * 0.002;

10141


10142
        /* '<S604>:1:32' */

10143
        _rtB->PS = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -

10144
          71000.0) * 9.3174939999999987E-6, 17.08161) * 3.956392;

10145


10146
        /* '<S604>:1:33' */

10147
        _rtB->rou = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -

10148
          71000.0) * 9.3174939999999987E-6, 16.08161) * 6.421057E-5;

10149
      } else {

10150
        /* '<S604>:1:35' */

10151
        T = 186.87;

10152


10153
        /* '<S604>:1:36' */

10154
        _rtB->PS = (1.042E-6 * TrigonometricFunction4_o + 2.273) * 0.101325 *

10155
          exp((87284.8 - TrigonometricFunction4_o) / 5470.0);

10156


10157
        /* '<S604>:1:37' */

10158
        _rtB->rou = exp((87284.8 - TrigonometricFunction4_o) / 5470.0) *

10159
          4.4603475E-6;

10160
      }

10161


10162
      /*  end */

10163
      /* '<S604>:1:45' */

10164
      _rtB->sound = 20.0468 * sqrt(T);

10165


10166
      /* '<S604>:1:46' */

10167
      TrigonometricFunction4_o = 6.356766E+6 / (6.356766E+6 + _rtB->Alt);

10168
      _rtB->g = TrigonometricFunction4_o * TrigonometricFunction4_o * 9.80665;

10169


10170
      /* End of MATLAB Function: '<S464>/Embedded MATLAB Function1' */

10171


10172
      /* Product: '<S464>/Product' */

10173
      _rtB->Product4_n = _rtB->rou;

10174


10175
      /* Product: '<S602>/Product2' */

10176
      _rtB->u2rhoV2 *= _rtB->Product4_n;

10177


10178
      /* Gain: '<S602>/1//2rhoV^2' */

10179
      _rtB->u2rhoV2 *= 0.5;

10180


10181
      /* Product: '<S480>/Product4' incorporates:

10182
       *  Constant: '<S480>/Constant2'

10183
       */

10184
      _rtB->Product4_n = _rtB->u2rhoV2 * 1.952;

10185


10186
      /* Selector: '<S876>/Selector1' */

10187
      _rtB->Product2_f[0] = _rtB->Product4[3];

10188


10189
      /* Gain: '<S876>/Gain1' */

10190
      _rtB->VectorConcatenate_b[0] = _rtB->Product2_f[0];

10191


10192
      /* Selector: '<S876>/Selector1' */

10193
      _rtB->Product2_f[1] = _rtB->Product4[0];

10194


10195
      /* Gain: '<S876>/Gain1' */

10196
      _rtB->VectorConcatenate_b[1] = _rtB->Product2_f[1];

10197


10198
      /* Selector: '<S876>/Selector1' */

10199
      _rtB->Product2_f[2] = _rtB->Product4[6];

10200


10201
      /* Gain: '<S876>/Gain1' */

10202
      _rtB->VectorConcatenate_b[2] = -_rtB->Product2_f[2];

10203


10204
      /* Selector: '<S876>/Selector2' */

10205
      _rtB->sincos_o1_m[0] = _rtB->Product4[7];

10206


10207
      /* Gain: '<S876>/Gain2' */

10208
      _rtB->VectorConcatenate_b[3] = _rtB->sincos_o1_m[0];

10209


10210
      /* Selector: '<S876>/Selector3' */

10211
      _rtB->sincos_o1_m[0] = _rtB->Product4[1];

10212


10213
      /* Gain: '<S876>/Gain3' */

10214
      _rtB->VectorConcatenate_b[5] = -_rtB->sincos_o1_m[0];

10215


10216
      /* Selector: '<S876>/Selector2' */

10217
      _rtB->sincos_o1_m[1] = _rtB->Product4[8];

10218


10219
      /* Gain: '<S876>/Gain2' */

10220
      _rtB->VectorConcatenate_b[4] = _rtB->sincos_o1_m[1];

10221


10222
      /* Selector: '<S876>/Selector3' */

10223
      _rtB->sincos_o1_m[1] = _rtB->Product4[4];

10224


10225
      /* Gain: '<S876>/Gain3' */

10226
      _rtB->VectorConcatenate_b[6] = _rtB->sincos_o1_m[1];

10227


10228
      /* If: '<S846>/If' */

10229
      if (ssIsMajorTimeStep(S)) {

10230
        rtAction = (int8_T)!((_rtB->VectorConcatenate_b[2] >= 1.0) ||

10231
                             (_rtB->VectorConcatenate_b[2] <= -1.0));

10232
        ((int8_T *)ssGetDWork(S, 507))[0] = rtAction;

10233
      } else {

10234
        rtAction = ((int8_T *)ssGetDWork(S, 507))[0];

10235
      }

10236


10237
      switch (rtAction) {

10238
       case 0:

10239
        /* Outputs for IfAction SubSystem: '<S846>/AxisRotZeroR3' incorporates:

10240
         *  ActionPort: '<S875>/Action Port'

10241
         */

10242
        /* Fcn: '<S875>/Fcn1' */

10243
        _rtB->Merge_n[0] = AFCS_MODEL1_rt_atan2d_snf(_rtB->VectorConcatenate_b[5],

10244
          _rtB->VectorConcatenate_b[6]);

10245


10246
        /* Fcn: '<S875>/Fcn2' */

10247
        if (_rtB->VectorConcatenate_b[2] > 1.0) {

10248
          TrigonometricFunction4_o = 1.0;

10249
        } else if (_rtB->VectorConcatenate_b[2] < -1.0) {

10250
          TrigonometricFunction4_o = -1.0;

10251
        } else {

10252
          TrigonometricFunction4_o = _rtB->VectorConcatenate_b[2];

10253
        }

10254


10255
        _rtB->Merge_n[1] = asin(TrigonometricFunction4_o);

10256


10257
        /* End of Fcn: '<S875>/Fcn2' */

10258


10259
        /* Fcn: '<S875>/Fcn3' */

10260
        _rtB->Merge_n[2] = 0.0;

10261


10262
        /* End of Outputs for SubSystem: '<S846>/AxisRotZeroR3' */

10263
        break;

10264


10265
       case 1:

10266
        /* Outputs for IfAction SubSystem: '<S846>/AxisRotDefault' incorporates:

10267
         *  ActionPort: '<S874>/Action Port'

10268
         */

10269
        /* Fcn: '<S874>/Fcn1' */

10270
        _rtB->Merge_n[0] = AFCS_MODEL1_rt_atan2d_snf(_rtB->VectorConcatenate_b[0],

10271
          _rtB->VectorConcatenate_b[1]);

10272


10273
        /* Fcn: '<S874>/Fcn2' */

10274
        if (_rtB->VectorConcatenate_b[2] > 1.0) {

10275
          TrigonometricFunction4_o = 1.0;

10276
        } else if (_rtB->VectorConcatenate_b[2] < -1.0) {

10277
          TrigonometricFunction4_o = -1.0;

10278
        } else {

10279
          TrigonometricFunction4_o = _rtB->VectorConcatenate_b[2];

10280
        }

10281


10282
        _rtB->Merge_n[1] = asin(TrigonometricFunction4_o);

10283


10284
        /* End of Fcn: '<S874>/Fcn2' */

10285


10286
        /* Fcn: '<S874>/Fcn3' */

10287
        _rtB->Merge_n[2] = AFCS_MODEL1_rt_atan2d_snf(_rtB->VectorConcatenate_b[3],

10288
          _rtB->VectorConcatenate_b[4]);

10289


10290
        /* End of Outputs for SubSystem: '<S846>/AxisRotDefault' */

10291
        break;

10292
      }

10293


10294
      /* End of If: '<S846>/If' */

10295


10296
      /* Switch: '<S607>/Switch' */

10297
      if (_rtB->UnitConversion_o[0] > 3.0) {

10298
        /* Trigonometry: '<S605>/Incidence' */

10299
        _rtB->Incidence = AFCS_MODEL1_rt_atan2d_snf(_rtB->Sum_m[2], _rtB->Sum_m

10300
          [0]);

10301
        _rtB->Switch_h = _rtB->Incidence;

10302
      } else {

10303
        _rtB->Switch_h = _rtB->Merge_n[1];

10304
      }

10305


10306
      /* End of Switch: '<S607>/Switch' */

10307


10308
      /* Gain: '<S482>/Unit Conversion' */

10309
      _rtB->table2D_Cn = 57.295779513082323 * _rtB->Switch_h;

10310


10311
      /* Saturate: '<S484>/Saturation1' */

10312
      if (_rtB->table2D_Cn > 20.0) {

10313
        _rtB->table2D_Cn = 20.0;

10314
      } else {

10315
        if (_rtB->table2D_Cn < -20.0) {

10316
          _rtB->table2D_Cn = -20.0;

10317
        }

10318
      }

10319


10320
      /* End of Saturate: '<S484>/Saturation1' */

10321


10322
      /* Gain: '<S588>/Unit Conversion' */

10323
      _rtB->add_d = 0.017453292519943295 * _rtB->table2D_Cn;

10324


10325
      /* Product: '<S612>/Product' */

10326
      _rtB->UnitConversion_j = _rtB->Sum_m[0] * _rtB->Sum_m[0];

10327


10328
      /* Product: '<S612>/Product1' */

10329
      _rtB->saturation = _rtB->Sum_m[1] * _rtB->Sum_m[1];

10330


10331
      /* Product: '<S612>/Product2' */

10332
      _rtB->table2D_Cm_dr = _rtB->Sum_m[2] * _rtB->Sum_m[2];

10333


10334
      /* Sum: '<S612>/Sum' */

10335
      _rtB->UnitConversion_j = (_rtB->UnitConversion_j + _rtB->saturation) +

10336
        _rtB->table2D_Cm_dr;

10337


10338
      /* Sqrt: '<S605>/Airspeed' */

10339
      _rtB->Airspeed = sqrt(_rtB->UnitConversion_j);

10340


10341
      /* Switch: '<S607>/Switch1' incorporates:

10342
       *  Constant: '<S607>/Constant'

10343
       */

10344
      if (_rtB->UnitConversion_o[0] > 3.0) {

10345
        /* Saturate: '<S605>/Saturation' */

10346
        if (_rtB->Airspeed <= 0.001) {

10347
          _rtB->Product_ei = 0.001;

10348
        } else {

10349
          _rtB->Product_ei = _rtB->Airspeed;

10350
        }

10351


10352
        /* End of Saturate: '<S605>/Saturation' */

10353


10354
        /* Product: '<S605>/Product' */

10355
        _rtB->Product_ei = _rtB->Sum_m[1] / _rtB->Product_ei;

10356


10357
        /* Trigonometry: '<S605>/Sideslip' */

10358
        if (_rtB->Product_ei > 1.0) {

10359
          TrigonometricFunction4_o = 1.0;

10360
        } else if (_rtB->Product_ei < -1.0) {

10361
          TrigonometricFunction4_o = -1.0;

10362
        } else {

10363
          TrigonometricFunction4_o = _rtB->Product_ei;

10364
        }

10365


10366
        _rtB->Sideslip = asin(TrigonometricFunction4_o);

10367


10368
        /* End of Trigonometry: '<S605>/Sideslip' */

10369
        _rtB->UnitConversion_j = _rtB->Sideslip;

10370
      } else {

10371
        _rtB->UnitConversion_j = 0.0;

10372
      }

10373


10374
      /* End of Switch: '<S607>/Switch1' */

10375


10376
      /* Gain: '<S481>/Unit Conversion' */

10377
      _rtB->saturation = 57.295779513082323 * _rtB->UnitConversion_j;

10378


10379
      /* Saturate: '<S484>/Saturation' */

10380
      if (_rtB->saturation > 20.0) {

10381
        _rtB->saturation = 20.0;

10382
      } else {

10383
        if (_rtB->saturation < -20.0) {

10384
          _rtB->saturation = -20.0;

10385
        }

10386
      }

10387


10388
      /* End of Saturate: '<S484>/Saturation' */

10389


10390
      /* Gain: '<S589>/Unit Conversion' */

10391
      _rtB->table2D_Cm_dr = 0.017453292519943295 * _rtB->saturation;

10392


10393
      /* SignalConversion: '<S590>/TmpSignal ConversionAtsincosInport1' */

10394
      _rtB->sincos_o1_m[0] = _rtB->add_d;

10395
      _rtB->sincos_o1_m[1] = _rtB->table2D_Cm_dr;

10396


10397
      /* Trigonometry: '<S590>/sincos' */

10398
      TrigonometricFunction4_o = cos(_rtB->sincos_o1_m[0]);

10399
      _rtB->sincos_o1_m[0] = sin(_rtB->sincos_o1_m[0]);

10400
      _rtB->sincos_o2_h[0] = TrigonometricFunction4_o;

10401
      TrigonometricFunction4_o = cos(_rtB->sincos_o1_m[1]);

10402
      _rtB->sincos_o1_m[1] = sin(_rtB->sincos_o1_m[1]);

10403
      _rtB->sincos_o2_h[1] = TrigonometricFunction4_o;

10404


10405
      /* Product: '<S591>/u(3)*u(4)' */

10406
      _rtB->VectorConcatenate_a[0] = _rtB->sincos_o2_h[0] * _rtB->sincos_o2_h[1];

10407


10408
      /* Product: '<S594>/u(2)*u(3)' */

10409
      _rtB->add_d = _rtB->sincos_o1_m[1] * _rtB->sincos_o2_h[0];

10410


10411
      /* UnaryMinus: '<S594>/Unary Minus' */

10412
      _rtB->VectorConcatenate_a[1] = -_rtB->add_d;

10413


10414
      /* UnaryMinus: '<S597>/Unary Minus' */

10415
      _rtB->VectorConcatenate_a[2] = -_rtB->sincos_o1_m[0];

10416


10417
      /* SignalConversion: '<S600>/ConcatBufferAtVector ConcatenateIn4' */

10418
      _rtB->VectorConcatenate_a[3] = _rtB->sincos_o1_m[1];

10419


10420
      /* SignalConversion: '<S600>/ConcatBufferAtVector ConcatenateIn5' */

10421
      _rtB->VectorConcatenate_a[4] = _rtB->sincos_o2_h[1];

10422


10423
      /* SignalConversion: '<S600>/ConcatBufferAtVector ConcatenateIn6' incorporates:

10424
       *  Constant: '<S598>/Constant'

10425
       */

10426
      _rtB->VectorConcatenate_a[5] = 0.0;

10427


10428
      /* Product: '<S593>/u(1)*u(4)' */

10429
      _rtB->VectorConcatenate_a[6] = _rtB->sincos_o1_m[0] * _rtB->sincos_o2_h[1];

10430


10431
      /* Product: '<S596>/u(1)*u(2)' */

10432
      _rtB->add_d = _rtB->sincos_o1_m[0] * _rtB->sincos_o1_m[1];

10433


10434
      /* UnaryMinus: '<S596>/Unary Minus' */

10435
      _rtB->VectorConcatenate_a[7] = -_rtB->add_d;

10436


10437
      /* SignalConversion: '<S600>/ConcatBufferAtVector ConcatenateIn9' */

10438
      _rtB->VectorConcatenate_a[8] = _rtB->sincos_o2_h[0];

10439


10440
      /* Math: '<S488>/Math Function' */

10441
      for (i_1 = 0; i_1 < 3; i_1++) {

10442
        _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_a[i_1];

10443
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_a[i_1 + 3];

10444
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_a[i_1 + 6];

10445
      }

10446


10447
      /* End of Math: '<S488>/Math Function' */

10448


10449
      /* Gain: '<S487>/Gain17' */

10450
      _rtB->table2D_Cn *= 0.017452006980802792;

10451


10452
      /* Product: '<S497>/product' incorporates:

10453
       *  Constant: '<S497>/value'

10454
       */

10455
      _rtB->add_d = _rtB->table2D_Cn * 57.29;

10456


10457
      /* Saturate: '<S497>/saturation' */

10458
      if (_rtB->add_d > 30.0) {

10459
        _rtB->add_d = 30.0;

10460
      } else {

10461
        if (_rtB->add_d < -6.0) {

10462
          _rtB->add_d = -6.0;

10463
        }

10464
      }

10465


10466
      /* End of Saturate: '<S497>/saturation' */

10467


10468
      /* Lookup_n-D: '<S491>/table1D_CD_base' */

10469
      _rtB->table2D_Cm_dr = AFCS_MODEL1_look1_binlxpw(_rtB->add_d,

10470
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled44, 18U);

10471


10472
      /* Sum: '<S483>/Add1' */

10473
      _rtB->table2D_Cn_dr -= _rtB->table2D_Cn_dar;

10474


10475
      /* Gain: '<S483>/Gain1' */

10476
      _rtB->table2D_Cn_dr *= 0.5;

10477


10478
      /* Gain: '<S463>/Gain4' */

10479
      _rtB->table2D_Cn_dar = -_rtB->table2D_Cn_dr;

10480


10481
      /* Gain: '<S487>/Gain1' */

10482
      _rtB->table2D_Cn_dar *= 0.017452006980802792;

10483


10484
      /* Product: '<S499>/product' incorporates:

10485
       *  Constant: '<S499>/value'

10486
       */

10487
      _rtB->table2D_Cn_dar *= 57.29;

10488


10489
      /* Saturate: '<S499>/saturation' */

10490
      if (_rtB->table2D_Cn_dar > 30.0) {

10491
        _rtB->table2D_Cn_dar = 30.0;

10492
      } else {

10493
        if (_rtB->table2D_Cn_dar < -30.0) {

10494
          _rtB->table2D_Cn_dar = -30.0;

10495
        }

10496
      }

10497


10498
      /* End of Saturate: '<S499>/saturation' */

10499


10500
      /* Lookup_n-D: '<S491>/table2D_CD_dal' */

10501
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,

10502
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10503
        AFCS_MODEL1_ConstP.pooled46, AFCS_MODEL1_ConstP.pooled138, 19U);

10504


10505
      /* Gain: '<S487>/Gain2' */

10506
      _rtB->table2D_Cn_dr *= 0.017452006980802792;

10507


10508
      /* Product: '<S500>/product' incorporates:

10509
       *  Constant: '<S500>/value'

10510
       */

10511
      _rtB->table2D_Cn_dr *= 57.29;

10512


10513
      /* Saturate: '<S500>/saturation' */

10514
      if (_rtB->table2D_Cn_dr > 30.0) {

10515
        _rtB->table2D_Cn_dr = 30.0;

10516
      } else {

10517
        if (_rtB->table2D_Cn_dr < -30.0) {

10518
          _rtB->table2D_Cn_dr = -30.0;

10519
        }

10520
      }

10521


10522
      /* End of Saturate: '<S500>/saturation' */

10523


10524
      /* Lookup_n-D: '<S491>/table2D_CD_dar' */

10525
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,

10526
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10527
        AFCS_MODEL1_ConstP.pooled46, AFCS_MODEL1_ConstP.pooled138, 19U);

10528


10529
      /* Lookup_n-D: '<S491>/table2D_CD_dfal' */

10530
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10531
        ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10532
        AFCS_MODEL1_ConstP.pooled48, AFCS_MODEL1_ConstP.pooled138, 19U);

10533


10534
      /* Lookup_n-D: '<S491>/table2D_CD_dfar' */

10535
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10536
        ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10537
        AFCS_MODEL1_ConstP.pooled48, AFCS_MODEL1_ConstP.pooled138, 19U);

10538


10539
      /* Sum: '<S483>/Add' */

10540
      _rtB->table2D_Cm_dfar += _rtB->table2D_Cn_dfal;

10541


10542
      /* Gain: '<S483>/Gain' */

10543
      _rtB->table2D_Cm_dfar *= 0.5;

10544


10545
      /* Gain: '<S487>/Gain3' */

10546
      _rtB->table2D_Cm_dfar *= 0.017452006980802792;

10547


10548
      /* Product: '<S501>/product' incorporates:

10549
       *  Constant: '<S501>/value'

10550
       */

10551
      _rtB->table2D_Cm_dfar *= 57.29;

10552


10553
      /* Saturate: '<S501>/saturation' */

10554
      if (_rtB->table2D_Cm_dfar > 30.0) {

10555
        _rtB->table2D_Cm_dfar = 30.0;

10556
      } else {

10557
        if (_rtB->table2D_Cm_dfar < -30.0) {

10558
          _rtB->table2D_Cm_dfar = -30.0;

10559
        }

10560
      }

10561


10562
      /* End of Saturate: '<S501>/saturation' */

10563


10564
      /* Lookup_n-D: '<S491>/table2D_CD_de' */

10565
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,

10566
        _rtB->table2D_Cm_dfar, AFCS_MODEL1_ConstP.pooled45,

10567
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled49,

10568
        AFCS_MODEL1_ConstP.pooled138, 19U);

10569


10570
      /* Gain: '<S487>/Gain4' */

10571
      _rtB->add_e *= 0.017452006980802792;

10572


10573
      /* Product: '<S504>/product' incorporates:

10574
       *  Constant: '<S504>/value'

10575
       */

10576
      _rtB->add_e *= 57.29;

10577


10578
      /* Saturate: '<S504>/saturation' */

10579
      if (_rtB->add_e > 30.0) {

10580
        _rtB->add_e = 30.0;

10581
      } else {

10582
        if (_rtB->add_e < -30.0) {

10583
          _rtB->add_e = -30.0;

10584
        }

10585
      }

10586


10587
      /* End of Saturate: '<S504>/saturation' */

10588


10589
      /* Lookup_n-D: '<S491>/table2D_CD_dr' */

10590
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,

10591
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10592
        AFCS_MODEL1_ConstP.pooled50, AFCS_MODEL1_ConstP.pooled138, 19U);

10593


10594
      /* Sum: '<S491>/add' */

10595
      _rtB->table2D_Cm_dr = (((((_rtB->table2D_Cm_dr + _rtB->add) + _rtB->add_b)

10596
        + _rtB->jxi) + _rtB->jxi_k) + _rtB->table2D_Cn_dfal) + _rtB->ixk;

10597


10598
      /* Lookup_n-D: '<S493>/table2D_CY_dal' */

10599
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,

10600
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10601
        AFCS_MODEL1_ConstP.pooled51, AFCS_MODEL1_ConstP.pooled138, 19U);

10602


10603
      /* Lookup_n-D: '<S493>/table2D_CY_dar' */

10604
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,

10605
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10606
        AFCS_MODEL1_ConstP.pooled52, AFCS_MODEL1_ConstP.pooled138, 19U);

10607


10608
      /* Lookup_n-D: '<S493>/table2D_CY_dfal' */

10609
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10610
        ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10611
        AFCS_MODEL1_ConstP.pooled53, AFCS_MODEL1_ConstP.pooled138, 19U);

10612


10613
      /* Lookup_n-D: '<S493>/table2D_CY_dfar' */

10614
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10615
        ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10616
        AFCS_MODEL1_ConstP.pooled54, AFCS_MODEL1_ConstP.pooled138, 19U);

10617


10618
      /* Lookup_n-D: '<S493>/table2D_CY_dr' */

10619
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,

10620
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10621
        AFCS_MODEL1_ConstP.pooled55, AFCS_MODEL1_ConstP.pooled138, 19U);

10622


10623
      /* Sum: '<S493>/add' */

10624
      _rtB->ixk = (((_rtB->ixk + _rtB->jxi_k) + _rtB->jxi) + _rtB->add_b) +

10625
        _rtB->add;

10626


10627
      /* Lookup_n-D: '<S492>/table1D_CL_base' */

10628
      _rtB->jxi_k = AFCS_MODEL1_look1_binlxpw(_rtB->add_d,

10629
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled56, 18U);

10630


10631
      /* Lookup_n-D: '<S492>/table2D_CL_de' */

10632
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cm_dfar,

10633
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10634
        AFCS_MODEL1_ConstP.pooled57, AFCS_MODEL1_ConstP.pooled138, 19U);

10635


10636
      /* Lookup_n-D: '<S492>/table2D_CL_dal' */

10637
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,

10638
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10639
        AFCS_MODEL1_ConstP.pooled58, AFCS_MODEL1_ConstP.pooled138, 19U);

10640


10641
      /* Lookup_n-D: '<S492>/table2D_CL_dar' */

10642
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,

10643
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10644
        AFCS_MODEL1_ConstP.pooled58, AFCS_MODEL1_ConstP.pooled138, 19U);

10645


10646
      /* Lookup_n-D: '<S492>/table2D_CL_dfal' */

10647
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,

10648
        AFCS_MODEL1_rtC(S)->saturation, AFCS_MODEL1_ConstP.pooled45,

10649
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,

10650
        AFCS_MODEL1_ConstP.pooled138, 19U);

10651


10652
      /* Lookup_n-D: '<S492>/table2D_CL_dfar' */

10653
      _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10654
        ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10655
        AFCS_MODEL1_ConstP.pooled59, AFCS_MODEL1_ConstP.pooled138, 19U);

10656


10657
      /* Lookup_n-D: '<S492>/table2D_CL_dr' */

10658
      _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,

10659
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10660
        AFCS_MODEL1_ConstP.pooled60, AFCS_MODEL1_ConstP.pooled138, 19U);

10661


10662
      /* Sum: '<S492>/add' */

10663
      _rtB->jxi_k = (((((_rtB->jxi_k + _rtB->jxi) + _rtB->add_b) + _rtB->add) +

10664
                      _rtB->table2D_Cn_dfal) + _rtB->jxi_p) + _rtB->ixk_a;

10665


10666
      /* Lookup_n-D: '<S494>/table2D_Cl_dal' */

10667
      _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,

10668
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10669
        AFCS_MODEL1_ConstP.pooled61, AFCS_MODEL1_ConstP.pooled138, 19U);

10670


10671
      /* Lookup_n-D: '<S494>/table2D_Cl_dar' */

10672
      _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,

10673
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10674
        AFCS_MODEL1_ConstP.pooled62, AFCS_MODEL1_ConstP.pooled138, 19U);

10675


10676
      /* Lookup_n-D: '<S494>/table2D_Cl_dr' */

10677
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,

10678
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10679
        AFCS_MODEL1_ConstP.pooled63, AFCS_MODEL1_ConstP.pooled138, 19U);

10680


10681
      /* Lookup_n-D: '<S494>/table2D_Cl_dfal' */

10682
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10683
        ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10684
        AFCS_MODEL1_ConstP.pooled64, AFCS_MODEL1_ConstP.pooled138, 19U);

10685


10686
      /* Lookup_n-D: '<S494>/table2D_Cl_dfar' */

10687
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10688
        ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10689
        AFCS_MODEL1_ConstP.pooled65, AFCS_MODEL1_ConstP.pooled138, 19U);

10690


10691
      /* Sum: '<S494>/add' */

10692
      _rtB->ixk_a = (((_rtB->ixk_a + _rtB->jxi_p) + _rtB->jxi) + _rtB->add_b) +

10693
        _rtB->add;

10694


10695
      /* Lookup_n-D: '<S495>/table1D_Cm_base' */

10696
      _rtB->jxi_p = AFCS_MODEL1_look1_binlxpw(_rtB->add_d,

10697
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled66, 18U);

10698


10699
      /* Lookup_n-D: '<S495>/table2D_Cm_de' */

10700
      _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,

10701
        _rtB->table2D_Cm_dfar, AFCS_MODEL1_ConstP.pooled45,

10702
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled67,

10703
        AFCS_MODEL1_ConstP.pooled138, 19U);

10704


10705
      /* Lookup_n-D: '<S495>/table2D_Cm_dal' */

10706
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,

10707
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10708
        AFCS_MODEL1_ConstP.pooled68, AFCS_MODEL1_ConstP.pooled138, 19U);

10709


10710
      /* Lookup_n-D: '<S495>/table2D_Cm_dar' */

10711
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,

10712
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10713
        AFCS_MODEL1_ConstP.pooled68, AFCS_MODEL1_ConstP.pooled138, 19U);

10714


10715
      /* Lookup_n-D: '<S495>/table2D_Cm_dfal' */

10716
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10717
        ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10718
        AFCS_MODEL1_ConstP.pooled69, AFCS_MODEL1_ConstP.pooled138, 19U);

10719


10720
      /* Lookup_n-D: '<S495>/table2D_Cm_dfar' */

10721
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,

10722
        AFCS_MODEL1_rtC(S)->saturation_c, AFCS_MODEL1_ConstP.pooled45,

10723
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,

10724
        AFCS_MODEL1_ConstP.pooled138, 19U);

10725


10726
      /* Lookup_n-D: '<S495>/table2D_Cm_dr' */

10727
      _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,

10728
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10729
        AFCS_MODEL1_ConstP.pooled70, AFCS_MODEL1_ConstP.pooled138, 19U);

10730


10731
      /* Sum: '<S495>/add' */

10732
      _rtB->jxi_p = (((((_rtB->jxi_p + _rtB->table2D_Cm_dfar) + _rtB->jxi) +

10733
                       _rtB->add_b) + _rtB->add) + _rtB->table2D_Cn_dfal) +

10734
        _rtB->kxj;

10735


10736
      /* Lookup_n-D: '<S496>/table2D_Cn_dal' */

10737
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,

10738
        _rtB->table2D_Cn_dar, AFCS_MODEL1_ConstP.pooled45,

10739
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled71,

10740
        AFCS_MODEL1_ConstP.pooled138, 19U);

10741


10742
      /* Lookup_n-D: '<S496>/table2D_Cn_dar' */

10743
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,

10744
        _rtB->table2D_Cn_dr, AFCS_MODEL1_ConstP.pooled45,

10745
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled72,

10746
        AFCS_MODEL1_ConstP.pooled138, 19U);

10747


10748
      /* Lookup_n-D: '<S496>/table2D_Cn_dr' */

10749
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,

10750
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10751
        AFCS_MODEL1_ConstP.pooled73, AFCS_MODEL1_ConstP.pooled138, 19U);

10752


10753
      /* Lookup_n-D: '<S496>/table2D_Cn_dfal' */

10754
      _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10755
        ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10756
        AFCS_MODEL1_ConstP.pooled74, AFCS_MODEL1_ConstP.pooled138, 19U);

10757


10758
      /* Lookup_n-D: '<S496>/table2D_Cn_dfar' */

10759
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)

10760
        ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10761
        AFCS_MODEL1_ConstP.pooled75, AFCS_MODEL1_ConstP.pooled138, 19U);

10762


10763
      /* Sum: '<S496>/add' */

10764
      _rtB->table2D_Cn_dar = (((_rtB->table2D_Cn_dar + _rtB->table2D_Cn_dr) +

10765
        _rtB->add_e) + _rtB->kxj) + _rtB->add_d;

10766


10767
      /* Product: '<S513>/product' incorporates:

10768
       *  Constant: '<S513>/value'

10769
       */

10770
      _rtB->kxj = _rtB->table2D_Cn * 57.29;

10771


10772
      /* Saturate: '<S513>/saturation' */

10773
      if (_rtB->kxj > 30.0) {

10774
        _rtB->kxj = 30.0;

10775
      } else {

10776
        if (_rtB->kxj < -6.0) {

10777
          _rtB->kxj = -6.0;

10778
        }

10779
      }

10780


10781
      /* End of Saturate: '<S513>/saturation' */

10782


10783
      /* Gain: '<S487>/Gain8' */

10784
      _rtB->saturation *= 0.017452006980802792;

10785


10786
      /* Product: '<S514>/product' incorporates:

10787
       *  Constant: '<S514>/value'

10788
       */

10789
      _rtB->jxi = _rtB->saturation * 57.29;

10790


10791
      /* Saturate: '<S514>/saturation' */

10792
      if (_rtB->jxi > 10.0) {

10793
        _rtB->jxi = 10.0;

10794
      } else {

10795
        if (_rtB->jxi < -10.0) {

10796
          _rtB->jxi = -10.0;

10797
        }

10798
      }

10799


10800
      /* End of Saturate: '<S514>/saturation' */

10801


10802
      /* Lookup_n-D: '<S507>/table2D_CD' */

10803
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,

10804
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

10805
        AFCS_MODEL1_ConstP.table2D_CD_tableData, AFCS_MODEL1_ConstP.pooled139,

10806
        19U);

10807


10808
      /* Lookup_n-D: '<S509>/table2D_CY' */

10809
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,

10810
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

10811
        AFCS_MODEL1_ConstP.table2D_CY_tableData, AFCS_MODEL1_ConstP.pooled139,

10812
        19U);

10813


10814
      /* Lookup_n-D: '<S508>/table2D_CL' */

10815
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,

10816
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

10817
        AFCS_MODEL1_ConstP.table2D_CL_tableData, AFCS_MODEL1_ConstP.pooled139,

10818
        19U);

10819


10820
      /* Lookup_n-D: '<S510>/table2D_Cl' */

10821
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,

10822
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

10823
        AFCS_MODEL1_ConstP.table2D_Cl_tableData, AFCS_MODEL1_ConstP.pooled139,

10824
        19U);

10825


10826
      /* Lookup_n-D: '<S511>/table2D_Cm' */

10827
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,

10828
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

10829
        AFCS_MODEL1_ConstP.table2D_Cm_tableData, AFCS_MODEL1_ConstP.pooled139,

10830
        19U);

10831


10832
      /* Lookup_n-D: '<S512>/table2D_Cn' */

10833
      _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,

10834
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

10835
        AFCS_MODEL1_ConstP.table2D_Cn_tableData, AFCS_MODEL1_ConstP.pooled139,

10836
        19U);

10837


10838
      /* Sum: '<S489>/Sum' */

10839
      _rtB->Sum2_g[0] = _rtB->table2D_Cm_dr + _rtB->add_b;

10840
      _rtB->Sum2_g[1] = _rtB->ixk + _rtB->add;

10841
      _rtB->Sum2_g[2] = _rtB->jxi_k + _rtB->add_d;

10842
      _rtB->Sum2_g[3] = _rtB->ixk_a + _rtB->add_e;

10843
      _rtB->Sum2_g[4] = _rtB->jxi_p + _rtB->table2D_Cn_dr;

10844
      _rtB->Sum2_g[5] = _rtB->table2D_Cn_dar + _rtB->kxj;

10845


10846
      /* SignalConversion: '<S490>/ConcatBufferAtMatrix Concatenate1In1' */

10847
      for (i_1 = 0; i_1 < 6; i_1++) {

10848
        _rtB->MatrixConcatenate1[i_1] = 0.0;

10849
      }

10850


10851
      /* End of SignalConversion: '<S490>/ConcatBufferAtMatrix Concatenate1In1' */

10852


10853
      /* Product: '<S581>/product' incorporates:

10854
       *  Constant: '<S581>/value'

10855
       */

10856
      _rtB->kxj = _rtB->table2D_Cn * 57.29;

10857


10858
      /* Saturate: '<S581>/saturation' */

10859
      if (_rtB->kxj > 30.0) {

10860
        _rtB->kxj = 30.0;

10861
      } else {

10862
        if (_rtB->kxj < -6.0) {

10863
          _rtB->kxj = -6.0;

10864
        }

10865
      }

10866


10867
      /* End of Saturate: '<S581>/saturation' */

10868


10869
      /* Lookup_n-D: '<S575>/table1D_CD_base' */

10870
      _rtB->ixk_a = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,

10871
        AFCS_MODEL1_ConstP.pooled45,

10872
        AFCS_MODEL1_ConstP.table1D_CD_base_tableData, 18U);

10873


10874
      /* Lookup_n-D: '<S575>/table2D_CD_dal' */

10875
      _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10876
        ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10877
        AFCS_MODEL1_ConstP.pooled78, AFCS_MODEL1_ConstP.pooled138, 19U);

10878


10879
      /* Lookup_n-D: '<S575>/table2D_CD_dar' */

10880
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10881
        ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10882
        AFCS_MODEL1_ConstP.pooled78, AFCS_MODEL1_ConstP.pooled138, 19U);

10883


10884
      /* Lookup_n-D: '<S575>/table2D_CD_de' */

10885
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10886
        ->saturation_k, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10887
        AFCS_MODEL1_ConstP.table2D_CD_de_tableData, AFCS_MODEL1_ConstP.pooled138,

10888
        19U);

10889


10890
      /* Lookup_n-D: '<S575>/table2D_CD_dr' */

10891
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10892
        ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10893
        AFCS_MODEL1_ConstP.table2D_CD_dr_tableData, AFCS_MODEL1_ConstP.pooled138,

10894
        19U);

10895


10896
      /* Sum: '<S575>/add' */

10897
      _rtB->ixk_a = (((_rtB->ixk_a + _rtB->jxi_p) + _rtB->ixk) + _rtB->jxi_k) +

10898
        _rtB->jxi;

10899


10900
      /* Lookup_n-D: '<S577>/table2D_CY_dal' */

10901
      _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10902
        ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10903
        AFCS_MODEL1_ConstP.table2D_CY_dal_tableData,

10904
        AFCS_MODEL1_ConstP.pooled138, 19U);

10905


10906
      /* Lookup_n-D: '<S577>/table2D_CY_dar' */

10907
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10908
        ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10909
        AFCS_MODEL1_ConstP.table2D_CY_dar_tableData,

10910
        AFCS_MODEL1_ConstP.pooled138, 19U);

10911


10912
      /* Lookup_n-D: '<S577>/table2D_CY_dr' */

10913
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10914
        ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10915
        AFCS_MODEL1_ConstP.table2D_CY_dr_tableData, AFCS_MODEL1_ConstP.pooled138,

10916
        19U);

10917


10918
      /* Sum: '<S577>/add' */

10919
      _rtB->jxi_p = (_rtB->jxi_p + _rtB->ixk) + _rtB->jxi_k;

10920


10921
      /* Lookup_n-D: '<S576>/table1D_CL_base' */

10922
      _rtB->ixk = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,

10923
        AFCS_MODEL1_ConstP.pooled45,

10924
        AFCS_MODEL1_ConstP.table1D_CL_base_tableData, 18U);

10925


10926
      /* Lookup_n-D: '<S576>/table2D_CL_dal' */

10927
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10928
        ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10929
        AFCS_MODEL1_ConstP.pooled79, AFCS_MODEL1_ConstP.pooled138, 19U);

10930


10931
      /* Lookup_n-D: '<S576>/table2D_CL_dar' */

10932
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10933
        ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10934
        AFCS_MODEL1_ConstP.pooled79, AFCS_MODEL1_ConstP.pooled138, 19U);

10935


10936
      /* Lookup_n-D: '<S576>/table2D_CL_de' */

10937
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10938
        ->saturation_k, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10939
        AFCS_MODEL1_ConstP.table2D_CL_de_tableData, AFCS_MODEL1_ConstP.pooled138,

10940
        19U);

10941


10942
      /* Sum: '<S576>/add' */

10943
      _rtB->ixk = ((_rtB->ixk + _rtB->jxi_k) + _rtB->jxi) + _rtB->add_b;

10944


10945
      /* Lookup_n-D: '<S578>/table2D_Cl_dal' */

10946
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10947
        ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10948
        AFCS_MODEL1_ConstP.table2D_Cl_dal_tableData,

10949
        AFCS_MODEL1_ConstP.pooled138, 19U);

10950


10951
      /* Lookup_n-D: '<S578>/table2D_Cl_dar' */

10952
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10953
        ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10954
        AFCS_MODEL1_ConstP.table2D_Cl_dar_tableData,

10955
        AFCS_MODEL1_ConstP.pooled138, 19U);

10956


10957
      /* Lookup_n-D: '<S578>/table2D_Cl_dr' */

10958
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10959
        ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10960
        AFCS_MODEL1_ConstP.table2D_Cl_dr_tableData, AFCS_MODEL1_ConstP.pooled138,

10961
        19U);

10962


10963
      /* Sum: '<S578>/add' */

10964
      _rtB->jxi_k = (_rtB->jxi_k + _rtB->jxi) + _rtB->add_b;

10965


10966
      /* Lookup_n-D: '<S579>/table1D_Cm_base' */

10967
      _rtB->jxi = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,

10968
        AFCS_MODEL1_ConstP.pooled45,

10969
        AFCS_MODEL1_ConstP.table1D_Cm_base_tableData, 18U);

10970


10971
      /* Lookup_n-D: '<S579>/table2D_Cm_de' */

10972
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10973
        ->saturation_k, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10974
        AFCS_MODEL1_ConstP.table2D_Cm_de_tableData, AFCS_MODEL1_ConstP.pooled138,

10975
        19U);

10976


10977
      /* Lookup_n-D: '<S579>/table2D_Cm_dal' */

10978
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10979
        ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10980
        AFCS_MODEL1_ConstP.pooled80, AFCS_MODEL1_ConstP.pooled138, 19U);

10981


10982
      /* Lookup_n-D: '<S579>/table2D_Cm_dar' */

10983
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10984
        ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10985
        AFCS_MODEL1_ConstP.pooled80, AFCS_MODEL1_ConstP.pooled138, 19U);

10986


10987
      /* Lookup_n-D: '<S579>/table2D_Cm_dr' */

10988
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10989
        ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

10990
        AFCS_MODEL1_ConstP.table2D_Cm_dr_tableData, AFCS_MODEL1_ConstP.pooled138,

10991
        19U);

10992


10993
      /* Sum: '<S579>/add' */

10994
      _rtB->jxi = (((_rtB->jxi + _rtB->add_b) + _rtB->add) + _rtB->add_d) +

10995
        _rtB->add_e;

10996


10997
      /* Lookup_n-D: '<S580>/table2D_Cn_dal' */

10998
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

10999
        ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11000
        AFCS_MODEL1_ConstP.pooled81, AFCS_MODEL1_ConstP.pooled138, 19U);

11001


11002
      /* Lookup_n-D: '<S580>/table2D_Cn_dar' */

11003
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11004
        ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11005
        AFCS_MODEL1_ConstP.pooled81, AFCS_MODEL1_ConstP.pooled138, 19U);

11006


11007
      /* Lookup_n-D: '<S580>/table2D_Cn_dr' */

11008
      _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11009
        ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11010
        AFCS_MODEL1_ConstP.table2D_Cn_dr_tableData, AFCS_MODEL1_ConstP.pooled138,

11011
        19U);

11012


11013
      /* Sum: '<S580>/add' */

11014
      _rtB->add_b = (_rtB->add_b + _rtB->add) + _rtB->kxj;

11015


11016
      /* Product: '<S528>/product' incorporates:

11017
       *  Constant: '<S528>/value'

11018
       */

11019
      _rtB->kxj = _rtB->table2D_Cn * 57.29;

11020


11021
      /* Saturate: '<S528>/saturation' */

11022
      if (_rtB->kxj > 30.0) {

11023
        _rtB->kxj = 30.0;

11024
      } else {

11025
        if (_rtB->kxj < -6.0) {

11026
          _rtB->kxj = -6.0;

11027
        }

11028
      }

11029


11030
      /* End of Saturate: '<S528>/saturation' */

11031


11032
      /* Lookup_n-D: '<S522>/table1D_CD_base' */

11033
      _rtB->add = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,

11034
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled44, 18U);

11035


11036
      /* Lookup_n-D: '<S522>/table2D_CD_dal' */

11037
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11038
        ->saturation_bo, AFCS_MODEL1_ConstP.pooled45,

11039
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled46,

11040
        AFCS_MODEL1_ConstP.pooled138, 19U);

11041


11042
      /* Lookup_n-D: '<S522>/table2D_CD_dar' */

11043
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11044
        ->saturation_n, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11045
        AFCS_MODEL1_ConstP.pooled46, AFCS_MODEL1_ConstP.pooled138, 19U);

11046


11047
      /* Lookup_n-D: '<S522>/table2D_CD_dfal' */

11048
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11049
        AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,

11050
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled48,

11051
        AFCS_MODEL1_ConstP.pooled138, 19U);

11052


11053
      /* Lookup_n-D: '<S522>/table2D_CD_dfar' */

11054
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC

11055
        (S)->saturation_l3, AFCS_MODEL1_ConstP.pooled45,

11056
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled48,

11057
        AFCS_MODEL1_ConstP.pooled138, 19U);

11058


11059
      /* Lookup_n-D: '<S522>/table2D_CD_de' */

11060
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11061
        AFCS_MODEL1_rtC(S)->saturation_k0, AFCS_MODEL1_ConstP.pooled45,

11062
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled49,

11063
        AFCS_MODEL1_ConstP.pooled138, 19U);

11064


11065
      /* Lookup_n-D: '<S522>/table2D_CD_dr' */

11066
      _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11067
        AFCS_MODEL1_rtC(S)->saturation_h, AFCS_MODEL1_ConstP.pooled45,

11068
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled50,

11069
        AFCS_MODEL1_ConstP.pooled138, 19U);

11070


11071
      /* Sum: '<S522>/add' */

11072
      _rtB->add = (((((_rtB->add + _rtB->add_d) + _rtB->add_e) +

11073
                     _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr) +

11074
                   _rtB->table2D_Cn_dfal) + _rtB->table2D_Cm_dfar;

11075


11076
      /* Lookup_n-D: '<S524>/table2D_CY_dal' */

11077
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11078
        ->saturation_bo, AFCS_MODEL1_ConstP.pooled45,

11079
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled51,

11080
        AFCS_MODEL1_ConstP.pooled138, 19U);

11081


11082
      /* Lookup_n-D: '<S524>/table2D_CY_dar' */

11083
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11084
        ->saturation_n, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11085
        AFCS_MODEL1_ConstP.pooled52, AFCS_MODEL1_ConstP.pooled138, 19U);

11086


11087
      /* Lookup_n-D: '<S524>/table2D_CY_dfal' */

11088
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11089
        AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,

11090
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled53,

11091
        AFCS_MODEL1_ConstP.pooled138, 19U);

11092


11093
      /* Lookup_n-D: '<S524>/table2D_CY_dfar' */

11094
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC

11095
        (S)->saturation_l3, AFCS_MODEL1_ConstP.pooled45,

11096
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled54,

11097
        AFCS_MODEL1_ConstP.pooled138, 19U);

11098


11099
      /* Lookup_n-D: '<S524>/table2D_CY_dr' */

11100
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11101
        AFCS_MODEL1_rtC(S)->saturation_h, AFCS_MODEL1_ConstP.pooled45,

11102
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled55,

11103
        AFCS_MODEL1_ConstP.pooled138, 19U);

11104


11105
      /* Sum: '<S524>/add' */

11106
      _rtB->add_d = (((_rtB->add_d + _rtB->add_e) + _rtB->table2D_Cn_dar) +

11107
                     _rtB->table2D_Cn_dr) + _rtB->table2D_Cn_dfal;

11108


11109
      /* Lookup_n-D: '<S523>/table1D_CL_base' */

11110
      _rtB->add_e = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,

11111
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled56, 18U);

11112


11113
      /* Lookup_n-D: '<S523>/table2D_CL_de' */

11114
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11115
        AFCS_MODEL1_rtC(S)->saturation_k0, AFCS_MODEL1_ConstP.pooled45,

11116
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled57,

11117
        AFCS_MODEL1_ConstP.pooled138, 19U);

11118


11119
      /* Lookup_n-D: '<S523>/table2D_CL_dal' */

11120
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC

11121
        (S)->saturation_bo, AFCS_MODEL1_ConstP.pooled45,

11122
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled58,

11123
        AFCS_MODEL1_ConstP.pooled138, 19U);

11124


11125
      /* Lookup_n-D: '<S523>/table2D_CL_dar' */

11126
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11127
        AFCS_MODEL1_rtC(S)->saturation_n, AFCS_MODEL1_ConstP.pooled45,

11128
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled58,

11129
        AFCS_MODEL1_ConstP.pooled138, 19U);

11130


11131
      /* Lookup_n-D: '<S523>/table2D_CL_dfal' */

11132
      _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11133
        AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,

11134
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,

11135
        AFCS_MODEL1_ConstP.pooled138, 19U);

11136


11137
      /* Lookup_n-D: '<S523>/table2D_CL_dfar' */

11138
      _rtB->table2D_Cm_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC

11139
        (S)->saturation_l3, AFCS_MODEL1_ConstP.pooled45,

11140
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,

11141
        AFCS_MODEL1_ConstP.pooled138, 19U);

11142


11143
      /* Lookup_n-D: '<S523>/table2D_CL_dr' */

11144
      _rtB->ixj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11145
        ->saturation_h, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11146
        AFCS_MODEL1_ConstP.pooled60, AFCS_MODEL1_ConstP.pooled138, 19U);

11147


11148
      /* Sum: '<S523>/add' */

11149
      _rtB->add_e = (((((_rtB->add_e + _rtB->table2D_Cn_dar) +

11150
                        _rtB->table2D_Cn_dr) + _rtB->table2D_Cn_dfal) +

11151
                      _rtB->table2D_Cm_dfar) + _rtB->table2D_Cm_dr) + _rtB->ixj;

11152


11153
      /* Lookup_n-D: '<S525>/table2D_Cl_dal' */

11154
      _rtB->ixj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11155
        ->saturation_bo, AFCS_MODEL1_ConstP.pooled45,

11156
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled61,

11157
        AFCS_MODEL1_ConstP.pooled138, 19U);

11158


11159
      /* Lookup_n-D: '<S525>/table2D_Cl_dar' */

11160
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11161
        AFCS_MODEL1_rtC(S)->saturation_n, AFCS_MODEL1_ConstP.pooled45,

11162
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled62,

11163
        AFCS_MODEL1_ConstP.pooled138, 19U);

11164


11165
      /* Lookup_n-D: '<S525>/table2D_Cl_dr' */

11166
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC

11167
        (S)->saturation_h, AFCS_MODEL1_ConstP.pooled45,

11168
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled63,

11169
        AFCS_MODEL1_ConstP.pooled138, 19U);

11170


11171
      /* Lookup_n-D: '<S525>/table2D_Cl_dfal' */

11172
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11173
        AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,

11174
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled64,

11175
        AFCS_MODEL1_ConstP.pooled138, 19U);

11176


11177
      /* Lookup_n-D: '<S525>/table2D_Cl_dfar' */

11178
      _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11179
        AFCS_MODEL1_rtC(S)->saturation_l3, AFCS_MODEL1_ConstP.pooled45,

11180
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled65,

11181
        AFCS_MODEL1_ConstP.pooled138, 19U);

11182


11183
      /* Sum: '<S525>/add' */

11184
      _rtB->ixj = (((_rtB->ixj + _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr) +

11185
                   _rtB->table2D_Cn_dfal) + _rtB->table2D_Cm_dfar;

11186


11187
      /* Lookup_n-D: '<S526>/table1D_Cm_base' */

11188
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,

11189
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled66, 18U);

11190


11191
      /* Lookup_n-D: '<S526>/table2D_Cm_de' */

11192
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC

11193
        (S)->saturation_k0, AFCS_MODEL1_ConstP.pooled45,

11194
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled67,

11195
        AFCS_MODEL1_ConstP.pooled138, 19U);

11196


11197
      /* Lookup_n-D: '<S526>/table2D_Cm_dal' */

11198
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11199
        AFCS_MODEL1_rtC(S)->saturation_bo, AFCS_MODEL1_ConstP.pooled45,

11200
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled68,

11201
        AFCS_MODEL1_ConstP.pooled138, 19U);

11202


11203
      /* Lookup_n-D: '<S526>/table2D_Cm_dar' */

11204
      _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11205
        AFCS_MODEL1_rtC(S)->saturation_n, AFCS_MODEL1_ConstP.pooled45,

11206
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled68,

11207
        AFCS_MODEL1_ConstP.pooled138, 19U);

11208


11209
      /* Lookup_n-D: '<S526>/table2D_Cm_dfal' */

11210
      _rtB->table2D_Cm_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC

11211
        (S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,

11212
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,

11213
        AFCS_MODEL1_ConstP.pooled138, 19U);

11214


11215
      /* Lookup_n-D: '<S526>/table2D_Cm_dfar' */

11216
      _rtB->kxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11217
        ->saturation_l3, AFCS_MODEL1_ConstP.pooled45,

11218
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,

11219
        AFCS_MODEL1_ConstP.pooled138, 19U);

11220


11221
      /* Lookup_n-D: '<S526>/table2D_Cm_dr' */

11222
      _rtB->jxk = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11223
        ->saturation_h, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11224
        AFCS_MODEL1_ConstP.pooled70, AFCS_MODEL1_ConstP.pooled138, 19U);

11225


11226
      /* Sum: '<S526>/add' */

11227
      _rtB->table2D_Cn_dar = (((((_rtB->table2D_Cn_dar + _rtB->table2D_Cn_dr) +

11228
        _rtB->table2D_Cn_dfal) + _rtB->table2D_Cm_dfar) + _rtB->table2D_Cm_dr) +

11229
        _rtB->kxi) + _rtB->jxk;

11230


11231
      /* Lookup_n-D: '<S527>/table2D_Cn_dal' */

11232
      _rtB->jxk = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11233
        ->saturation_bo, AFCS_MODEL1_ConstP.pooled45,

11234
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled71,

11235
        AFCS_MODEL1_ConstP.pooled138, 19U);

11236


11237
      /* Lookup_n-D: '<S527>/table2D_Cn_dar' */

11238
      _rtB->kxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11239
        ->saturation_n, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11240
        AFCS_MODEL1_ConstP.pooled72, AFCS_MODEL1_ConstP.pooled138, 19U);

11241


11242
      /* Lookup_n-D: '<S527>/table2D_Cn_dr' */

11243
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC

11244
        (S)->saturation_h, AFCS_MODEL1_ConstP.pooled45,

11245
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled73,

11246
        AFCS_MODEL1_ConstP.pooled138, 19U);

11247


11248
      /* Lookup_n-D: '<S527>/table2D_Cn_dfal' */

11249
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,

11250
        AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,

11251
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled74,

11252
        AFCS_MODEL1_ConstP.pooled138, 19U);

11253


11254
      /* Lookup_n-D: '<S527>/table2D_Cn_dfar' */

11255
      _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)

11256
        ->saturation_l3, AFCS_MODEL1_ConstP.pooled45,

11257
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled75,

11258
        AFCS_MODEL1_ConstP.pooled138, 19U);

11259


11260
      /* Sum: '<S527>/add' */

11261
      _rtB->jxk = (((_rtB->jxk + _rtB->kxi) + _rtB->table2D_Cn_dr) +

11262
                   _rtB->table2D_Cn_dfal) + _rtB->kxj;

11263


11264
      /* Sum: '<S490>/Subtract' */

11265
      _rtB->MatrixConcatenate1[6] = _rtB->ixk_a - _rtB->add;

11266
      _rtB->MatrixConcatenate1[7] = _rtB->jxi_p - _rtB->add_d;

11267
      _rtB->MatrixConcatenate1[8] = _rtB->ixk - _rtB->add_e;

11268
      _rtB->MatrixConcatenate1[9] = _rtB->jxi_k - _rtB->ixj;

11269
      _rtB->MatrixConcatenate1[10] = _rtB->jxi - _rtB->table2D_Cn_dar;

11270
      _rtB->MatrixConcatenate1[11] = _rtB->add_b - _rtB->jxk;

11271


11272
      /* Product: '<S558>/product' incorporates:

11273
       *  Constant: '<S558>/value'

11274
       */

11275
      _rtB->jxk = _rtB->table2D_Cn * 57.29;

11276


11277
      /* Saturate: '<S558>/saturation' */

11278
      if (_rtB->jxk > 30.0) {

11279
        _rtB->jxk = 30.0;

11280
      } else {

11281
        if (_rtB->jxk < -6.0) {

11282
          _rtB->jxk = -6.0;

11283
        }

11284
      }

11285


11286
      /* End of Saturate: '<S558>/saturation' */

11287


11288
      /* Lookup_n-D: '<S552>/table1D_CD_base' */

11289
      _rtB->kxi = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,

11290
        AFCS_MODEL1_ConstP.pooled45,

11291
        AFCS_MODEL1_ConstP.table1D_CD_base_tableData_a, 18U);

11292


11293
      /* Lookup_n-D: '<S552>/table2D_CD_dal' */

11294
      _rtB->ixj = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11295
        ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,

11296
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled83,

11297
        AFCS_MODEL1_ConstP.pooled138, 19U);

11298


11299
      /* Lookup_n-D: '<S552>/table2D_CD_dar' */

11300
      _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11301
        ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11302
        AFCS_MODEL1_ConstP.pooled83, AFCS_MODEL1_ConstP.pooled138, 19U);

11303


11304
      /* Lookup_n-D: '<S552>/table2D_CD_de' */

11305
      _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11306
        ->saturation_d, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11307
        AFCS_MODEL1_ConstP.table2D_CD_de_tableData_b,

11308
        AFCS_MODEL1_ConstP.pooled138, 19U);

11309


11310
      /* Lookup_n-D: '<S552>/table2D_CD_dr' */

11311
      _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11312
        ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,

11313
        AFCS_MODEL1_ConstP.pooled47,

11314
        AFCS_MODEL1_ConstP.table2D_CD_dr_tableData_o,

11315
        AFCS_MODEL1_ConstP.pooled138, 19U);

11316


11317
      /* Sum: '<S552>/add' */

11318
      _rtB->kxi = (((_rtB->kxi + _rtB->ixj) + _rtB->kxj) + _rtB->ixk_a) +

11319
        _rtB->jxi_p;

11320


11321
      /* Lookup_n-D: '<S554>/table2D_CY_dal' */

11322
      _rtB->ixj = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11323
        ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,

11324
        AFCS_MODEL1_ConstP.pooled47,

11325
        AFCS_MODEL1_ConstP.table2D_CY_dal_tableData_e,

11326
        AFCS_MODEL1_ConstP.pooled138, 19U);

11327


11328
      /* Lookup_n-D: '<S554>/table2D_CY_dar' */

11329
      _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11330
        ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11331
        AFCS_MODEL1_ConstP.table2D_CY_dar_tableData_l,

11332
        AFCS_MODEL1_ConstP.pooled138, 19U);

11333


11334
      /* Lookup_n-D: '<S554>/table2D_CY_dr' */

11335
      _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11336
        ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,

11337
        AFCS_MODEL1_ConstP.pooled47,

11338
        AFCS_MODEL1_ConstP.table2D_CY_dr_tableData_g,

11339
        AFCS_MODEL1_ConstP.pooled138, 19U);

11340


11341
      /* Sum: '<S554>/add' */

11342
      _rtB->ixj = (_rtB->ixj + _rtB->kxj) + _rtB->ixk_a;

11343


11344
      /* Lookup_n-D: '<S553>/table1D_CL_base' */

11345
      _rtB->kxj = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,

11346
        AFCS_MODEL1_ConstP.pooled45,

11347
        AFCS_MODEL1_ConstP.table1D_CL_base_tableData_l, 18U);

11348


11349
      /* Lookup_n-D: '<S553>/table2D_CL_dal' */

11350
      _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11351
        ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,

11352
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled84,

11353
        AFCS_MODEL1_ConstP.pooled138, 19U);

11354


11355
      /* Lookup_n-D: '<S553>/table2D_CL_dar' */

11356
      _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11357
        ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11358
        AFCS_MODEL1_ConstP.pooled84, AFCS_MODEL1_ConstP.pooled138, 19U);

11359


11360
      /* Lookup_n-D: '<S553>/table2D_CL_de' */

11361
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11362
        ->saturation_d, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11363
        AFCS_MODEL1_ConstP.table2D_CL_de_tableData_d,

11364
        AFCS_MODEL1_ConstP.pooled138, 19U);

11365


11366
      /* Sum: '<S553>/add' */

11367
      _rtB->kxj = ((_rtB->kxj + _rtB->ixk_a) + _rtB->jxi_p) + _rtB->ixk;

11368


11369
      /* Lookup_n-D: '<S555>/table2D_Cl_dal' */

11370
      _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11371
        ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,

11372
        AFCS_MODEL1_ConstP.pooled47,

11373
        AFCS_MODEL1_ConstP.table2D_Cl_dal_tableData_n,

11374
        AFCS_MODEL1_ConstP.pooled138, 19U);

11375


11376
      /* Lookup_n-D: '<S555>/table2D_Cl_dar' */

11377
      _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11378
        ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11379
        AFCS_MODEL1_ConstP.table2D_Cl_dar_tableData_k,

11380
        AFCS_MODEL1_ConstP.pooled138, 19U);

11381


11382
      /* Lookup_n-D: '<S555>/table2D_Cl_dr' */

11383
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11384
        ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,

11385
        AFCS_MODEL1_ConstP.pooled47,

11386
        AFCS_MODEL1_ConstP.table2D_Cl_dr_tableData_h,

11387
        AFCS_MODEL1_ConstP.pooled138, 19U);

11388


11389
      /* Sum: '<S555>/add' */

11390
      _rtB->ixk_a = (_rtB->ixk_a + _rtB->jxi_p) + _rtB->ixk;

11391


11392
      /* Lookup_n-D: '<S556>/table1D_Cm_base' */

11393
      _rtB->jxi_p = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,

11394
        AFCS_MODEL1_ConstP.pooled45,

11395
        AFCS_MODEL1_ConstP.table1D_Cm_base_tableData_l, 18U);

11396


11397
      /* Lookup_n-D: '<S556>/table2D_Cm_de' */

11398
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11399
        ->saturation_d, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11400
        AFCS_MODEL1_ConstP.table2D_Cm_de_tableData_h,

11401
        AFCS_MODEL1_ConstP.pooled138, 19U);

11402


11403
      /* Lookup_n-D: '<S556>/table2D_Cm_dal' */

11404
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11405
        ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,

11406
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled85,

11407
        AFCS_MODEL1_ConstP.pooled138, 19U);

11408


11409
      /* Lookup_n-D: '<S556>/table2D_Cm_dar' */

11410
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11411
        ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11412
        AFCS_MODEL1_ConstP.pooled85, AFCS_MODEL1_ConstP.pooled138, 19U);

11413


11414
      /* Lookup_n-D: '<S556>/table2D_Cm_dr' */

11415
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11416
        ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,

11417
        AFCS_MODEL1_ConstP.pooled47,

11418
        AFCS_MODEL1_ConstP.table2D_Cm_dr_tableData_f,

11419
        AFCS_MODEL1_ConstP.pooled138, 19U);

11420


11421
      /* Sum: '<S556>/add' */

11422
      _rtB->jxi_p = (((_rtB->jxi_p + _rtB->ixk) + _rtB->jxi_k) + _rtB->jxi) +

11423
        _rtB->add_b;

11424


11425
      /* Lookup_n-D: '<S557>/table2D_Cn_dal' */

11426
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11427
        ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,

11428
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.table2D_Cn_dal_tableData,

11429
        AFCS_MODEL1_ConstP.pooled138, 19U);

11430


11431
      /* Lookup_n-D: '<S557>/table2D_Cn_dar' */

11432
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11433
        ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11434
        AFCS_MODEL1_ConstP.table2D_Cn_dar_tableData,

11435
        AFCS_MODEL1_ConstP.pooled138, 19U);

11436


11437
      /* Lookup_n-D: '<S557>/table2D_Cn_dr' */

11438
      _rtB->jxk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11439
        ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,

11440
        AFCS_MODEL1_ConstP.pooled47,

11441
        AFCS_MODEL1_ConstP.table2D_Cn_dr_tableData_a,

11442
        AFCS_MODEL1_ConstP.pooled138, 19U);

11443


11444
      /* Sum: '<S557>/add' */

11445
      _rtB->ixk = (_rtB->ixk + _rtB->jxi_k) + _rtB->jxk;

11446


11447
      /* Product: '<S543>/product' incorporates:

11448
       *  Constant: '<S543>/value'

11449
       */

11450
      _rtB->jxk = _rtB->table2D_Cn * 57.29;

11451


11452
      /* Saturate: '<S543>/saturation' */

11453
      if (_rtB->jxk > 30.0) {

11454
        _rtB->jxk = 30.0;

11455
      } else {

11456
        if (_rtB->jxk < -6.0) {

11457
          _rtB->jxk = -6.0;

11458
        }

11459
      }

11460


11461
      /* End of Saturate: '<S543>/saturation' */

11462


11463
      /* Lookup_n-D: '<S537>/table1D_CD_base' */

11464
      _rtB->jxi_k = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,

11465
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled44, 18U);

11466


11467
      /* Lookup_n-D: '<S537>/table2D_CD_dal' */

11468
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11469
        ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11470
        AFCS_MODEL1_ConstP.pooled46, AFCS_MODEL1_ConstP.pooled138, 19U);

11471


11472
      /* Lookup_n-D: '<S537>/table2D_CD_dar' */

11473
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11474
        ->saturation_gs, AFCS_MODEL1_ConstP.pooled45,

11475
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled46,

11476
        AFCS_MODEL1_ConstP.pooled138, 19U);

11477


11478
      /* Lookup_n-D: '<S537>/table2D_CD_dfal' */

11479
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11480
        ->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,

11481
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled48,

11482
        AFCS_MODEL1_ConstP.pooled138, 19U);

11483


11484
      /* Lookup_n-D: '<S537>/table2D_CD_dfar' */

11485
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11486
        ->saturation_f, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11487
        AFCS_MODEL1_ConstP.pooled48, AFCS_MODEL1_ConstP.pooled138, 19U);

11488


11489
      /* Lookup_n-D: '<S537>/table2D_CD_de' */

11490
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11491
        ->saturation_ng, AFCS_MODEL1_ConstP.pooled45,

11492
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled49,

11493
        AFCS_MODEL1_ConstP.pooled138, 19U);

11494


11495
      /* Lookup_n-D: '<S537>/table2D_CD_dr' */

11496
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11497
        AFCS_MODEL1_rtC(S)->saturation_by, AFCS_MODEL1_ConstP.pooled45,

11498
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled50,

11499
        AFCS_MODEL1_ConstP.pooled138, 19U);

11500


11501
      /* Sum: '<S537>/add' */

11502
      _rtB->jxi_k = (((((_rtB->jxi_k + _rtB->jxi) + _rtB->add_b) + _rtB->add) +

11503
                      _rtB->add_d) + _rtB->add_e) + _rtB->table2D_Cn_dar;

11504


11505
      /* Lookup_n-D: '<S539>/table2D_CY_dal' */

11506
      _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11507
        ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11508
        AFCS_MODEL1_ConstP.pooled51, AFCS_MODEL1_ConstP.pooled138, 19U);

11509


11510
      /* Lookup_n-D: '<S539>/table2D_CY_dar' */

11511
      _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11512
        ->saturation_gs, AFCS_MODEL1_ConstP.pooled45,

11513
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled52,

11514
        AFCS_MODEL1_ConstP.pooled138, 19U);

11515


11516
      /* Lookup_n-D: '<S539>/table2D_CY_dfal' */

11517
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11518
        ->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,

11519
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled53,

11520
        AFCS_MODEL1_ConstP.pooled138, 19U);

11521


11522
      /* Lookup_n-D: '<S539>/table2D_CY_dfar' */

11523
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11524
        ->saturation_f, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11525
        AFCS_MODEL1_ConstP.pooled54, AFCS_MODEL1_ConstP.pooled138, 19U);

11526


11527
      /* Lookup_n-D: '<S539>/table2D_CY_dr' */

11528
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11529
        ->saturation_by, AFCS_MODEL1_ConstP.pooled45,

11530
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled55,

11531
        AFCS_MODEL1_ConstP.pooled138, 19U);

11532


11533
      /* Sum: '<S539>/add' */

11534
      _rtB->jxi = (((_rtB->jxi + _rtB->add_b) + _rtB->add) + _rtB->add_d) +

11535
        _rtB->add_e;

11536


11537
      /* Lookup_n-D: '<S538>/table1D_CL_base' */

11538
      _rtB->add_b = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,

11539
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled56, 18U);

11540


11541
      /* Lookup_n-D: '<S538>/table2D_CL_de' */

11542
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11543
        ->saturation_ng, AFCS_MODEL1_ConstP.pooled45,

11544
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled57,

11545
        AFCS_MODEL1_ConstP.pooled138, 19U);

11546


11547
      /* Lookup_n-D: '<S538>/table2D_CL_dal' */

11548
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11549
        ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11550
        AFCS_MODEL1_ConstP.pooled58, AFCS_MODEL1_ConstP.pooled138, 19U);

11551


11552
      /* Lookup_n-D: '<S538>/table2D_CL_dar' */

11553
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11554
        ->saturation_gs, AFCS_MODEL1_ConstP.pooled45,

11555
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled58,

11556
        AFCS_MODEL1_ConstP.pooled138, 19U);

11557


11558
      /* Lookup_n-D: '<S538>/table2D_CL_dfal' */

11559
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11560
        AFCS_MODEL1_rtC(S)->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,

11561
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,

11562
        AFCS_MODEL1_ConstP.pooled138, 19U);

11563


11564
      /* Lookup_n-D: '<S538>/table2D_CL_dfar' */

11565
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC

11566
        (S)->saturation_f, AFCS_MODEL1_ConstP.pooled45,

11567
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,

11568
        AFCS_MODEL1_ConstP.pooled138, 19U);

11569


11570
      /* Lookup_n-D: '<S538>/table2D_CL_dr' */

11571
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11572
        AFCS_MODEL1_rtC(S)->saturation_by, AFCS_MODEL1_ConstP.pooled45,

11573
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled60,

11574
        AFCS_MODEL1_ConstP.pooled138, 19U);

11575


11576
      /* Sum: '<S538>/add' */

11577
      _rtB->add_b = (((((_rtB->add_b + _rtB->add) + _rtB->add_d) + _rtB->add_e)

11578
                      + _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr) +

11579
        _rtB->table2D_Cn_dfal;

11580


11581
      /* Lookup_n-D: '<S540>/table2D_Cl_dal' */

11582
      _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11583
        ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11584
        AFCS_MODEL1_ConstP.pooled61, AFCS_MODEL1_ConstP.pooled138, 19U);

11585


11586
      /* Lookup_n-D: '<S540>/table2D_Cl_dar' */

11587
      _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11588
        ->saturation_gs, AFCS_MODEL1_ConstP.pooled45,

11589
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled62,

11590
        AFCS_MODEL1_ConstP.pooled138, 19U);

11591


11592
      /* Lookup_n-D: '<S540>/table2D_Cl_dr' */

11593
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11594
        ->saturation_by, AFCS_MODEL1_ConstP.pooled45,

11595
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled63,

11596
        AFCS_MODEL1_ConstP.pooled138, 19U);

11597


11598
      /* Lookup_n-D: '<S540>/table2D_Cl_dfal' */

11599
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11600
        AFCS_MODEL1_rtC(S)->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,

11601
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled64,

11602
        AFCS_MODEL1_ConstP.pooled138, 19U);

11603


11604
      /* Lookup_n-D: '<S540>/table2D_Cl_dfar' */

11605
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC

11606
        (S)->saturation_f, AFCS_MODEL1_ConstP.pooled45,

11607
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled65,

11608
        AFCS_MODEL1_ConstP.pooled138, 19U);

11609


11610
      /* Sum: '<S540>/add' */

11611
      _rtB->add = (((_rtB->add + _rtB->add_d) + _rtB->add_e) +

11612
                   _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr;

11613


11614
      /* Lookup_n-D: '<S541>/table1D_Cm_base' */

11615
      _rtB->add_d = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,

11616
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled66, 18U);

11617


11618
      /* Lookup_n-D: '<S541>/table2D_Cm_de' */

11619
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11620
        ->saturation_ng, AFCS_MODEL1_ConstP.pooled45,

11621
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled67,

11622
        AFCS_MODEL1_ConstP.pooled138, 19U);

11623


11624
      /* Lookup_n-D: '<S541>/table2D_Cm_dal' */

11625
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11626
        AFCS_MODEL1_rtC(S)->saturation_g, AFCS_MODEL1_ConstP.pooled45,

11627
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled68,

11628
        AFCS_MODEL1_ConstP.pooled138, 19U);

11629


11630
      /* Lookup_n-D: '<S541>/table2D_Cm_dar' */

11631
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC

11632
        (S)->saturation_gs, AFCS_MODEL1_ConstP.pooled45,

11633
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled68,

11634
        AFCS_MODEL1_ConstP.pooled138, 19U);

11635


11636
      /* Lookup_n-D: '<S541>/table2D_Cm_dfal' */

11637
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11638
        AFCS_MODEL1_rtC(S)->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,

11639
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,

11640
        AFCS_MODEL1_ConstP.pooled138, 19U);

11641


11642
      /* Lookup_n-D: '<S541>/table2D_Cm_dfar' */

11643
      _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11644
        AFCS_MODEL1_rtC(S)->saturation_f, AFCS_MODEL1_ConstP.pooled45,

11645
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,

11646
        AFCS_MODEL1_ConstP.pooled138, 19U);

11647


11648
      /* Lookup_n-D: '<S541>/table2D_Cm_dr' */

11649
      _rtB->table2D_Cm_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC

11650
        (S)->saturation_by, AFCS_MODEL1_ConstP.pooled45,

11651
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled70,

11652
        AFCS_MODEL1_ConstP.pooled138, 19U);

11653


11654
      /* Sum: '<S541>/add' */

11655
      _rtB->add_d = (((((_rtB->add_d + _rtB->add_e) + _rtB->table2D_Cn_dar) +

11656
                       _rtB->table2D_Cn_dr) + _rtB->table2D_Cn_dfal) +

11657
                     _rtB->table2D_Cm_dfar) + _rtB->table2D_Cm_dr;

11658


11659
      /* Lookup_n-D: '<S542>/table2D_Cn_dal' */

11660
      _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11661
        ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11662
        AFCS_MODEL1_ConstP.pooled71, AFCS_MODEL1_ConstP.pooled138, 19U);

11663


11664
      /* Lookup_n-D: '<S542>/table2D_Cn_dar' */

11665
      _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11666
        AFCS_MODEL1_rtC(S)->saturation_gs, AFCS_MODEL1_ConstP.pooled45,

11667
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled72,

11668
        AFCS_MODEL1_ConstP.pooled138, 19U);

11669


11670
      /* Lookup_n-D: '<S542>/table2D_Cn_dr' */

11671
      _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC

11672
        (S)->saturation_by, AFCS_MODEL1_ConstP.pooled45,

11673
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled73,

11674
        AFCS_MODEL1_ConstP.pooled138, 19U);

11675


11676
      /* Lookup_n-D: '<S542>/table2D_Cn_dfal' */

11677
      _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,

11678
        AFCS_MODEL1_rtC(S)->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,

11679
        AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled74,

11680
        AFCS_MODEL1_ConstP.pooled138, 19U);

11681


11682
      /* Lookup_n-D: '<S542>/table2D_Cn_dfar' */

11683
      _rtB->jxk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)

11684
        ->saturation_f, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,

11685
        AFCS_MODEL1_ConstP.pooled75, AFCS_MODEL1_ConstP.pooled138, 19U);

11686


11687
      /* Sum: '<S542>/add' */

11688
      _rtB->add_e = (((_rtB->add_e + _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr)

11689
                     + _rtB->table2D_Cn_dfal) + _rtB->jxk;

11690


11691
      /* Sum: '<S490>/Subtract1' */

11692
      _rtB->MatrixConcatenate1[12] = _rtB->kxi - _rtB->jxi_k;

11693
      _rtB->MatrixConcatenate1[13] = _rtB->ixj - _rtB->jxi;

11694
      _rtB->MatrixConcatenate1[14] = _rtB->kxj - _rtB->add_b;

11695
      _rtB->MatrixConcatenate1[15] = _rtB->ixk_a - _rtB->add;

11696
      _rtB->MatrixConcatenate1[16] = _rtB->jxi_p - _rtB->add_d;

11697
      _rtB->MatrixConcatenate1[17] = _rtB->ixk - _rtB->add_e;

11698


11699
      /* Selector: '<S490>/Selector' */

11700
      _rtB->Product2_f[0] = _rtB->MatrixConcatenate1[0];

11701
      _rtB->Product2_f[1] = _rtB->MatrixConcatenate1[6];

11702
      _rtB->Product2_f[2] = _rtB->MatrixConcatenate1[12];

11703


11704
      /* Dynamic Look-Up Table Block: '<S490>/Lookup Table Dynamic'

11705
       * Input0  Data Type:  Floating Point real_T

11706
       * Input1  Data Type:  Floating Point real_T

11707
       * Input2  Data Type:  Floating Point real_T

11708
       * Output0 Data Type:  Floating Point real_T

11709
       * Lookup Method: Linear_Endpoint

11710
       *

11711
       */

11712
      AFCS_MODEL1_sf_LookUpE_4Ol9QRGv( &(_rtB->jxk), &_rtB->Product2_f[0],

11713
        AFCS_MODEL1_rtC(S)->Saturation, 0.0, 2U, 15.0);

11714


11715
      /* Selector: '<S490>/Selector1' */

11716
      _rtB->Product2_f[0] = _rtB->MatrixConcatenate1[2];

11717
      _rtB->Product2_f[1] = _rtB->MatrixConcatenate1[8];

11718
      _rtB->Product2_f[2] = _rtB->MatrixConcatenate1[14];

11719


11720
      /* Dynamic Look-Up Table Block: '<S490>/Lookup Table Dynamic1'

11721
       * Input0  Data Type:  Floating Point real_T

11722
       * Input1  Data Type:  Floating Point real_T

11723
       * Input2  Data Type:  Floating Point real_T

11724
       * Output0 Data Type:  Floating Point real_T

11725
       * Lookup Method: Linear_Endpoint

11726
       *

11727
       */

11728
      AFCS_MODEL1_sf_LookUpE_4Ol9QRGv( &(_rtB->kxi), &_rtB->Product2_f[0],

11729
        AFCS_MODEL1_rtC(S)->Saturation, 0.0, 2U, 15.0);

11730


11731
      /* Selector: '<S490>/Selector2' */

11732
      _rtB->Product2_f[0] = _rtB->MatrixConcatenate1[4];

11733
      _rtB->Product2_f[1] = _rtB->MatrixConcatenate1[10];

11734
      _rtB->Product2_f[2] = _rtB->MatrixConcatenate1[16];

11735


11736
      /* Dynamic Look-Up Table Block: '<S490>/Lookup Table Dynamic2'

11737
       * Input0  Data Type:  Floating Point real_T

11738
       * Input1  Data Type:  Floating Point real_T

11739
       * Input2  Data Type:  Floating Point real_T

11740
       * Output0 Data Type:  Floating Point real_T

11741
       * Lookup Method: Linear_Endpoint

11742
       *

11743
       */

11744
      AFCS_MODEL1_sf_LookUpE_4Ol9QRGv( &(_rtB->ixj), &_rtB->Product2_f[0],

11745
        AFCS_MODEL1_rtC(S)->Saturation, 0.0, 2U, 15.0);

11746


11747
      /* Product: '<S571>/product' incorporates:

11748
       *  Constant: '<S571>/value'

11749
       */

11750
      _rtB->table2D_Cn *= 57.29;

11751


11752
      /* Saturate: '<S571>/saturation' */

11753
      if (_rtB->table2D_Cn > 30.0) {

11754
        _rtB->table2D_Cn = 30.0;

11755
      } else {

11756
        if (_rtB->table2D_Cn < -6.0) {

11757
          _rtB->table2D_Cn = -6.0;

11758
        }

11759
      }

11760


11761
      /* End of Saturate: '<S571>/saturation' */

11762


11763
      /* Product: '<S572>/product' incorporates:

11764
       *  Constant: '<S572>/value'

11765
       */

11766
      _rtB->saturation *= 57.29;

11767


11768
      /* Saturate: '<S572>/saturation' */

11769
      if (_rtB->saturation > 10.0) {

11770
        _rtB->saturation = 10.0;

11771
      } else {

11772
        if (_rtB->saturation < -10.0) {

11773
          _rtB->saturation = -10.0;

11774
        }

11775
      }

11776


11777
      /* End of Saturate: '<S572>/saturation' */

11778


11779
      /* Lookup_n-D: '<S565>/table2D_CD' */

11780
      _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,

11781
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

11782
        AFCS_MODEL1_ConstP.table2D_CD_tableData_j, AFCS_MODEL1_ConstP.pooled139,

11783
        19U);

11784


11785
      /* Lookup_n-D: '<S567>/table2D_CY' */

11786
      _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,

11787
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

11788
        AFCS_MODEL1_ConstP.table2D_CY_tableData_j, AFCS_MODEL1_ConstP.pooled139,

11789
        19U);

11790


11791
      /* Lookup_n-D: '<S566>/table2D_CL' */

11792
      _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,

11793
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

11794
        AFCS_MODEL1_ConstP.table2D_CL_tableData_e, AFCS_MODEL1_ConstP.pooled139,

11795
        19U);

11796


11797
      /* Lookup_n-D: '<S568>/table2D_Cl' */

11798
      _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,

11799
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

11800
        AFCS_MODEL1_ConstP.table2D_Cl_tableData_k, AFCS_MODEL1_ConstP.pooled139,

11801
        19U);

11802


11803
      /* Lookup_n-D: '<S569>/table2D_Cm' */

11804
      _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,

11805
        AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,

11806
        AFCS_MODEL1_ConstP.table2D_Cm_tableData_h, AFCS_MODEL1_ConstP.pooled139,

11807
        19U);

11808


11809
      /* Lookup_n-D: '<S570>/table2D_Cn' */

11810
      _rtB->table2D_Cn = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn,

11811
        _rtB->saturation, AFCS_MODEL1_ConstP.pooled45,

11812
        AFCS_MODEL1_ConstP.pooled76, AFCS_MODEL1_ConstP.table2D_Cn_tableData_d,

11813
        AFCS_MODEL1_ConstP.pooled139, 19U);

11814


11815
      /* Sum: '<S490>/Sum' */

11816
      _rtB->Sum1_h[0] = _rtB->jxk + _rtB->kxj;

11817
      _rtB->Sum1_h[1] = _rtB->ixk_a;

11818
      _rtB->Sum1_h[2] = _rtB->kxi + _rtB->jxi_p;

11819
      _rtB->Sum1_h[3] = _rtB->ixk;

11820
      _rtB->Sum1_h[4] = _rtB->ixj + _rtB->jxi_k;

11821
      _rtB->Sum1_h[5] = _rtB->table2D_Cn;

11822
      for (i_1 = 0; i_1 < 6; i_1++) {

11823
        /* Sum: '<S487>/Sum' */

11824
        _rtB->Sum2_g[i_1] += _rtB->Sum1_h[i_1];

11825
      }

11826


11827
      /* Gain: '<S488>/Gain1' */

11828
      _rtB->jxk = -_rtB->Sum2_g[0];

11829


11830
      /* Gain: '<S488>/Gain' */

11831
      _rtB->kxi = -_rtB->Sum2_g[2];

11832


11833
      /* SignalConversion: '<S488>/TmpSignal ConversionAtProductInport2' incorporates:

11834
       *  Constant: '<S488>/Constant'

11835
       */

11836
      _rtB->Product2_f[0] = _rtB->jxk;

11837
      _rtB->Product2_f[1] = 0.0;

11838
      _rtB->Product2_f[2] = _rtB->kxi;

11839
      for (i_1 = 0; i_1 < 3; i_1++) {

11840
        /* Product: '<S488>/Product' */

11841
        _rtB->Sum4[i_1] = 0.0;

11842
        _rtB->Sum4[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->Product2_f[0];

11843
        _rtB->Sum4[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->Product2_f[1];

11844
        _rtB->Sum4[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->Product2_f[2];

11845


11846
        /* Sum: '<S486>/Sum' */

11847
        tmp[i_1 + 3] = _rtB->Sum2_g[3 + i_1];

11848
      }

11849


11850
      /* Sum: '<S486>/Sum' */

11851
      tmp[0] = _rtB->Sum4[0];

11852
      tmp[1] = _rtB->Sum2_g[1];

11853
      tmp[2] = _rtB->Sum4[2];

11854
      for (i_1 = 0; i_1 < 6; i_1++) {

11855
        _rtB->Sum2_g[i_1] = tmp[i_1];

11856
      }

11857


11858
      /* Gain: '<S480>/Gain' */

11859
      _rtB->Sum4[0] = 4.785 * _rtB->Sum2_g[3];

11860


11861
      /* Product: '<S480>/Product5' */

11862
      _rtB->Sum4[0] *= _rtB->Product4_n;

11863


11864
      /* Product: '<S480>/Product2' */

11865
      _rtB->Product2_f[0] = _rtB->Sum2_g[0] * _rtB->Product4_n;

11866


11867
      /* Gain: '<S480>/Gain' */

11868
      _rtB->Sum4[1] = 0.648 * _rtB->Sum2_g[4];

11869


11870
      /* Product: '<S480>/Product5' */

11871
      _rtB->Sum4[1] *= _rtB->Product4_n;

11872


11873
      /* Product: '<S480>/Product2' */

11874
      _rtB->Product2_f[1] = _rtB->Sum2_g[1] * _rtB->Product4_n;

11875


11876
      /* Gain: '<S480>/Gain' */

11877
      _rtB->Sum4[2] = 4.785 * _rtB->Sum2_g[5];

11878


11879
      /* Product: '<S480>/Product5' */

11880
      _rtB->Sum4[2] *= _rtB->Product4_n;

11881


11882
      /* Product: '<S480>/Product2' */

11883
      _rtB->Product2_f[2] = _rtB->Sum2_g[2] * _rtB->Product4_n;

11884


11885
      /* Product: '<S601>/Element product' */

11886
      _rtB->Sum1_h[0] = AFCS_MODEL1_rtC(S)->Gain2[1] * _rtB->Product2_f[2];

11887
      _rtB->Sum1_h[1] = AFCS_MODEL1_rtC(S)->Gain2[2] * _rtB->Product2_f[0];

11888
      _rtB->Sum1_h[2] = AFCS_MODEL1_rtC(S)->Gain2[0] * _rtB->Product2_f[1];

11889
      _rtB->Sum1_h[3] = AFCS_MODEL1_rtC(S)->Gain2[2] * _rtB->Product2_f[1];

11890
      _rtB->Sum1_h[4] = AFCS_MODEL1_rtC(S)->Gain2[0] * _rtB->Product2_f[2];

11891
      _rtB->Sum1_h[5] = AFCS_MODEL1_rtC(S)->Gain2[1] * _rtB->Product2_f[0];

11892


11893
      /* Sum: '<S601>/Add3' */

11894
      _rtB->Product2_p[0] = _rtB->Sum1_h[0] - _rtB->Sum1_h[3];

11895


11896
      /* Sum: '<S485>/Sum1' */

11897
      _rtB->Sum1[0] = _rtB->Sum4[0] + _rtB->Product2_p[0];

11898


11899
      /* Sum: '<S601>/Add3' */

11900
      _rtB->Product2_p[1] = _rtB->Sum1_h[1] - _rtB->Sum1_h[4];

11901


11902
      /* Sum: '<S485>/Sum1' */

11903
      _rtB->Sum1[1] = _rtB->Sum4[1] + _rtB->Product2_p[1];

11904


11905
      /* Sum: '<S601>/Add3' */

11906
      _rtB->Product2_p[2] = _rtB->Sum1_h[2] - _rtB->Sum1_h[5];

11907


11908
      /* Sum: '<S485>/Sum1' */

11909
      _rtB->Sum1[2] = _rtB->Sum4[2] + _rtB->Product2_p[2];

11910
    }

11911


11912
    if (ssIsSampleHit(S, 1, 0)) {

11913
    }

11914


11915
    if (1) {

11916
      /* Backlash: '<S446>/Backlash' */

11917
      if (_rtB->DataTypeConversion4[7] < ((real_T *)ssGetDWork(S, 4))[0] - 1.0)

11918
      {

11919
        _rtB->Backlash = _rtB->DataTypeConversion4[7] + 1.0;

11920
      } else if (_rtB->DataTypeConversion4[7] <= ((real_T *)ssGetDWork(S, 4))[0]

11921
                 + 1.0) {

11922
        _rtB->Backlash = ((real_T *)ssGetDWork(S, 4))[0];

11923
      } else {

11924
        _rtB->Backlash = _rtB->DataTypeConversion4[7] - 1.0;

11925
      }

11926


11927
      /* End of Backlash: '<S446>/Backlash' */

11928


11929
      /* Gain: '<S469>/Gain2' */

11930
      _rtB->jxk = 57.3 * _rtB->Merge_n[2];

11931


11932
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

11933
      _rtB->phiL = _rtB->jxk;

11934


11935
      /* Gain: '<S446>/Gain' */

11936
      _rtB->Lat_p = _rtB->UnitConversion_c4[0];

11937


11938
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

11939
      _rtB->Lat_j = _rtB->Lat_p;

11940


11941
      /* Gain: '<S446>/Gain1' */

11942
      _rtB->Lon_j = _rtB->UnitConversion_c4[1];

11943


11944
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

11945
      _rtB->Lon_e = _rtB->Lon_j;

11946


11947
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

11948
      _rtB->Altitude_j = _rtB->Alt;

11949


11950
      /* Math: '<S469>/Math Function' */

11951
      for (i_1 = 0; i_1 < 3; i_1++) {

11952
        _rtB->MathFunction1_a[3 * i_1] = _rtB->Product4[i_1];

11953
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->Product4[i_1 + 3];

11954
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->Product4[i_1 + 6];

11955
      }

11956


11957
      /* End of Math: '<S469>/Math Function' */

11958
      for (i_1 = 0; i_1 < 3; i_1++) {

11959
        /* Product: '<S469>/Product' */

11960
        _rtB->Product2_p[i_1] = 0.0;

11961


11962
        /* Product: '<S470>/Product3' */

11963
        _rtB->Sum4[i_1] = 0.0;

11964


11965
        /* Product: '<S469>/Product' */

11966
        _rtB->Product2_p[i_1] += _rtB->MathFunction1_a[i_1] *

11967
          _rtB->UnitConversion_o[0];

11968


11969
        /* Product: '<S470>/Product3' */

11970
        _rtB->Sum4[i_1] += _rtB->VectorConcatenate_n[i_1] * 0.0;

11971


11972
        /* Product: '<S469>/Product' */

11973
        _rtB->Product2_p[i_1] += _rtB->MathFunction1_a[i_1 + 3] *

11974
          _rtB->UnitConversion_o[1];

11975


11976
        /* Product: '<S470>/Product3' */

11977
        _rtB->Sum4[i_1] += _rtB->VectorConcatenate_n[i_1 + 3] * 0.0;

11978


11979
        /* Product: '<S469>/Product' */

11980
        _rtB->Product2_p[i_1] += _rtB->MathFunction1_a[i_1 + 6] *

11981
          _rtB->UnitConversion_o[2];

11982


11983
        /* Product: '<S470>/Product3' */

11984
        _rtB->Sum4[i_1] += _rtB->VectorConcatenate_n[i_1 + 6] * 7.292115E-5;

11985
      }

11986


11987
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

11988
      _rtB->GPS_vnorth_i = _rtB->Product2_p[0];

11989


11990
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

11991
      _rtB->GPS_veast_k = _rtB->Product2_p[1];

11992


11993
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

11994
      _rtB->GPS_vdown_k = _rtB->Product2_p[2];

11995


11996
      /* Gain: '<S469>/Gain3' */

11997
      _rtB->jxk = 57.3 * _rtB->Merge_n[1];

11998


11999
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12000
      _rtB->thetaL = _rtB->jxk;

12001


12002
      /* Gain: '<S767>/Unit Conversion' */

12003
      _rtB->jxk = 57.295779513082323 * _rtB->Switch_h;

12004


12005
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12006
      _rtB->Alfa = _rtB->jxk;

12007


12008
      /* Gain: '<S768>/Unit Conversion' */

12009
      _rtB->UnitConversion_j *= 57.295779513082323;

12010


12011
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12012
      _rtB->Beta = _rtB->UnitConversion_j;

12013


12014
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12015
      _rtB->Qbar = _rtB->u2rhoV2;

12016


12017
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12018
      _rtB->Va = _rtB->Airspeed;

12019


12020
      /* Gain: '<S469>/Gain4' */

12021
      _rtB->jxk = 57.3 * _rtB->Merge_n[0];

12022


12023
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12024
      _rtB->psiL = _rtB->jxk;

12025


12026
      /* Math: '<S783>/Math Function2' */

12027
      for (i_1 = 0; i_1 < 3; i_1++) {

12028
        _rtB->MathFunction1_a[3 * i_1] = _rtB->Product4[i_1];

12029
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->Product4[i_1 + 3];

12030
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->Product4[i_1 + 6];

12031
      }

12032


12033
      /* End of Math: '<S783>/Math Function2' */

12034


12035
      /* Product: '<S783>/Product3' */

12036
      for (i_1 = 0; i_1 < 3; i_1++) {

12037
        _rtB->Sum2_h[i_1] = 0.0;

12038
        _rtB->Sum2_h[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->ubvbwb[0];

12039
        _rtB->Sum2_h[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->ubvbwb[1];

12040
        _rtB->Sum2_h[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->ubvbwb[2];

12041
      }

12042


12043
      /* End of Product: '<S783>/Product3' */

12044


12045
      /* Gain: '<S950>/Unit Conversion' */

12046
      _rtB->jxk = 0.017453292519943295 * _rtB->UnitConversion_c4[0];

12047


12048
      /* Trigonometry: '<S783>/sine' */

12049
      _rtB->kxi = sin(_rtB->jxk);

12050


12051
      /* Product: '<S783>/Product' */

12052
      _rtB->kxi = _rtB->kxi * _rtB->kxi * AFCS_MODEL1_rtC(S)->Sum2;

12053


12054
      /* Sum: '<S783>/Sum2' incorporates:

12055
       *  Constant: '<S783>/f2'

12056
       */

12057
      _rtB->kxi = 1.0 - _rtB->kxi;

12058


12059
      /* Sqrt: '<S952>/sqrt' */

12060
      _rtB->ixj = sqrt(_rtB->kxi);

12061


12062
      /* Product: '<S952>/N' incorporates:

12063
       *  Constant: '<S952>/f4'

12064
       */

12065
      _rtB->ixj = 6.371E+6 / _rtB->ixj;

12066


12067
      /* Sum: '<S952>/Sum4' */

12068
      _rtB->ixj += _rtB->Sum3_j;

12069


12070
      /* Product: '<S783>/w1' */

12071
      _rtB->ixj = _rtB->Sum2_h[1] / _rtB->ixj;

12072


12073
      /* Math: '<S951>/Math Function' incorporates:

12074
       *  Constant: '<S951>/f3'

12075
       */

12076
      if (_rtB->kxi < 0.0) {

12077
        _rtB->kxi = -AFCS_MODEL1_rt_powd_snf(-_rtB->kxi, 1.5);

12078
      } else {

12079
        _rtB->kxi = AFCS_MODEL1_rt_powd_snf(_rtB->kxi, 1.5);

12080
      }

12081


12082
      /* End of Math: '<S951>/Math Function' */

12083


12084
      /* Product: '<S951>/M' incorporates:

12085
       *  Constant: '<S951>/f1'

12086
       */

12087
      _rtB->kxi = 6.371E+6 * AFCS_MODEL1_rtC(S)->Sum_ha / _rtB->kxi;

12088


12089
      /* Sum: '<S951>/Sum1' */

12090
      _rtB->kxi += _rtB->Sum3_j;

12091


12092
      /* Product: '<S783>/w2' */

12093
      _rtB->kxi = _rtB->Sum2_h[0] / _rtB->kxi;

12094


12095
      /* Gain: '<S783>/Gain' */

12096
      _rtB->kxi = -_rtB->kxi;

12097


12098
      /* Trigonometry: '<S783>/tan' */

12099
      _rtB->jxk = tan(_rtB->jxk);

12100


12101
      /* Product: '<S783>/w3' */

12102
      _rtB->jxk *= _rtB->ixj;

12103


12104
      /* Gain: '<S783>/Gain1' */

12105
      _rtB->jxk = -_rtB->jxk;

12106


12107
      /* SignalConversion: '<S470>/TmpSignal ConversionAtProduct2Inport2' */

12108
      _rtB->Sum2_h[0] = _rtB->ixj;

12109
      _rtB->Sum2_h[1] = _rtB->kxi;

12110
      _rtB->Sum2_h[2] = _rtB->jxk;

12111
      for (i_1 = 0; i_1 < 3; i_1++) {

12112
        /* Product: '<S470>/Product2' */

12113
        _rtB->Sum1_d[i_1] = 0.0;

12114
        _rtB->Sum1_d[i_1] += _rtB->Product4[i_1] * _rtB->Sum2_h[0];

12115
        _rtB->Sum1_d[i_1] += _rtB->Product4[i_1 + 3] * _rtB->Sum2_h[1];

12116
        _rtB->Sum1_d[i_1] += _rtB->Product4[i_1 + 6] * _rtB->Sum2_h[2];

12117


12118
        /* Sum: '<S470>/Sum3' */

12119
        _rtB->Sum1_d[i_1] += _rtB->Sum4[i_1];

12120


12121
        /* Sum: '<S470>/Sum2' */

12122
        _rtB->Sum2_op[i_1] = _rtB->Sum1_d[i_1];

12123
      }

12124


12125
      /* Integrator: '<S470>/p,q,r ' */

12126
      if (((int_T *)ssGetDWork(S, 347))[0] != 0) {

12127
        _rtX->pqr_CSTATE[0] = _rtB->Sum2_op[0];

12128
        _rtX->pqr_CSTATE[1] = _rtB->Sum2_op[1];

12129
        _rtX->pqr_CSTATE[2] = _rtB->Sum2_op[2];

12130
      }

12131


12132
      _rtB->Sum2_h[0] = _rtX->pqr_CSTATE[0];

12133


12134
      /* Sum: '<S470>/Sum1' */

12135
      _rtB->Sum1_d[0] = _rtB->Sum2_h[0] - _rtB->Sum1_d[0];

12136


12137
      /* Integrator: '<S470>/p,q,r ' */

12138
      _rtB->Sum2_h[1] = _rtX->pqr_CSTATE[1];

12139


12140
      /* Sum: '<S470>/Sum1' */

12141
      _rtB->Sum1_d[1] = _rtB->Sum2_h[1] - _rtB->Sum1_d[1];

12142


12143
      /* Integrator: '<S470>/p,q,r ' */

12144
      _rtB->Sum2_h[2] = _rtX->pqr_CSTATE[2];

12145


12146
      /* Sum: '<S470>/Sum1' */

12147
      _rtB->Sum1_d[2] = _rtB->Sum2_h[2] - _rtB->Sum1_d[2];

12148


12149
      /* Gain: '<S469>/Gain5' */

12150
      _rtB->jxk = 57.3 * _rtB->Sum1_d[0];

12151


12152
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12153
      _rtB->p = _rtB->jxk;

12154


12155
      /* Gain: '<S469>/Gain7' */

12156
      _rtB->jxk = 57.3 * _rtB->Sum1_d[1];

12157


12158
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12159
      _rtB->q = _rtB->jxk;

12160


12161
      /* Gain: '<S469>/Gain8' */

12162
      _rtB->jxk = 57.3 * _rtB->Sum1_d[2];

12163


12164
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

12165
      _rtB->r = _rtB->jxk;

12166


12167
      /* SignalConversion: '<S954>/TmpSignal ConversionAtSelectorInport1' */

12168
      _rtB->p_p[0] = _rtB->Lat_p;

12169
      _rtB->p_p[1] = _rtB->Lon_j;

12170
      _rtB->p_p[2] = _rtB->Alt;

12171


12172
      /* Selector: '<S954>/Selector2' */

12173
      _rtB->jxk = _rtB->p_p[0];

12174


12175
      /* Gain: '<S956>/Unit Conversion' */

12176
      _rtB->jxk *= 0.017453292519943295;

12177


12178
      /* Selector: '<S954>/Selector1' */

12179
      _rtB->kxi = _rtB->p_p[1];

12180


12181
      /* Gain: '<S957>/Unit Conversion' */

12182
      _rtB->kxi *= 0.017453292519943295;

12183


12184
      /* Selector: '<S954>/Selector' */

12185
      _rtB->ixj = _rtB->p_p[2];

12186


12187
      /* S-Function Block: <S954>/WGS84 Gravity S-Function */

12188
      {

12189
        int_T i;

12190
        real_T GM, opt_m2ft, deg2rad;

12191
        real_T *phi_ptr, *height_ptr;

12192
        boolean_T phi_wrapped = false;

12193
        real_T *phi = (real_T *) &((real_T*) ssGetDWork(S, 6))[0];

12194
        real_T *h = (real_T *) &((real_T*) ssGetDWork(S, 5))[0];

12195
        height_ptr = (real_T *) &_rtB->ixj;

12196
        phi_ptr = (real_T *) &_rtB->jxk;

12197


12198
        /* get unit conversion factor  */

12199
        opt_m2ft = 1.0;

12200
        deg2rad = 1.0;

12201


12202
        /* Use Earth's Atmosphere in Gravitational Const? */

12203
        GM = ( 1.0 == 0 ) ? WGS84_GM_DEF : WGS84_GM_PRM;

12204


12205
        /* WGS84TAYLORSERIES */

12206
        {

12207
          for (i = 0; i < 1; i++ ) {

12208
            real_T fphi, pi_2;

12209


12210
            /* create short variables for latitude (phi) and height (h) */

12211
            phi[i] = phi_ptr[i] * deg2rad;

12212
            h[i] = height_ptr[i] / opt_m2ft;

12213
            if (phi[i] > AERO_PI || phi[i] < -AERO_PI) {

12214
              phi[i] = fmod(phi[i]+AERO_PI, 2.0*AERO_PI) - AERO_PI;

12215
            }

12216


12217
            fphi = fabs(phi[i]);

12218
            pi_2 = AERO_PI/2.0;

12219
            if (fphi > pi_2 ) {

12220
              real_T sign = phi[i]/fphi;

12221
              phi_wrapped = true;

12222
              phi[i] = sign*(pi_2 - (fphi - pi_2));

12223
            } else {

12224
              phi_wrapped = false;

12225
            }

12226
          }

12227


12228
          wgs84_taylor_series(h, phi, opt_m2ft, &_rtB->MatrixConcatenate[2], 1);

12229


12230
          /* north and east components are zero */

12231
          _rtB->MatrixConcatenate[0] = ((real_T*) ssGetDWork(S, 9))[0];

12232
          _rtB->MatrixConcatenate[1] = ((real_T*) ssGetDWork(S, 9))[0];

12233
        }

12234
      }

12235


12236
      /* Math: '<S471>/Math Function' */

12237
      _rtB->p_p[0] = _rtB->MatrixConcatenate[0];

12238
      _rtB->p_p[1] = _rtB->MatrixConcatenate[1];

12239
      _rtB->p_p[2] = _rtB->MatrixConcatenate[2];

12240


12241
      /* Gain: '<S771>/Unit Conversion' */

12242
      _rtB->jxk = 0.017453292519943295 * _rtB->Lat_p;

12243


12244
      /* Trigonometry: '<S770>/Trigonometric Function1' */

12245
      _rtB->kxi = sin(_rtB->jxk);

12246


12247
      /* Math: '<S770>/Math Function' */

12248
      _rtB->ixj = _rtB->kxi * _rtB->kxi;

12249


12250
      /* Product: '<S770>/Product6' incorporates:

12251
       *  Constant: '<S770>/Constant1'

12252
       */

12253
      _rtB->ixj *= 0.0;

12254


12255
      /* Sum: '<S770>/Sum2' incorporates:

12256
       *  Constant: '<S770>/Constant2'

12257
       */

12258
      _rtB->ixj = 1.0 - _rtB->ixj;

12259


12260
      /* Product: '<S770>/Divide2' incorporates:

12261
       *  Constant: '<S770>/Constant'

12262
       */

12263
      _rtB->ixj /= 6.371E+6;

12264


12265
      /* Product: '<S770>/Divide1' */

12266
      _rtB->ixj *= _rtB->Product2_p[1];

12267


12268
      /* Math: '<S770>/Math Function1' */

12269
      _rtB->kxi *= _rtB->kxi;

12270


12271
      /* Product: '<S770>/Product7' incorporates:

12272
       *  Constant: '<S770>/Constant4'

12273
       */

12274
      _rtB->kxi *= 0.0;

12275


12276
      /* Gain: '<S770>/Gain4' */

12277
      _rtB->kxi *= 3.0;

12278


12279
      /* Sum: '<S770>/Sum3' incorporates:

12280
       *  Constant: '<S770>/Constant5'

12281
       */

12282
      _rtB->kxi = (AFCS_MODEL1_rtC(S)->Gain3 + 1.0) - _rtB->kxi;

12283


12284
      /* Product: '<S770>/Divide3' incorporates:

12285
       *  Constant: '<S770>/Constant3'

12286
       */

12287
      _rtB->kxi /= 6.371E+6;

12288


12289
      /* Product: '<S770>/Divide' */

12290
      _rtB->kxi *= _rtB->Product2_p[0];

12291


12292
      /* Gain: '<S770>/Gain1' */

12293
      _rtB->kxi = -_rtB->kxi;

12294


12295
      /* Trigonometry: '<S770>/Trigonometric Function' */

12296
      _rtB->jxk = tan(_rtB->jxk);

12297


12298
      /* Product: '<S770>/Product4' */

12299
      _rtB->jxk *= _rtB->ixj;

12300


12301
      /* Gain: '<S770>/Gain2' */

12302
      _rtB->jxk = -_rtB->jxk;

12303


12304
      /* SignalConversion: '<S770>/TmpSignal ConversionAtProduct5Inport2' */

12305
      _rtB->Product2_p[0] = _rtB->ixj;

12306
      _rtB->Product2_p[1] = _rtB->kxi;

12307
      _rtB->Product2_p[2] = _rtB->jxk;

12308
      for (i_1 = 0; i_1 < 3; i_1++) {

12309
        /* Product: '<S770>/Product1' */

12310
        _rtB->Product1_f[i_1] = 0.0;

12311


12312
        /* Product: '<S770>/Product5' */

12313
        _rtB->Sum_mx[i_1] = 0.0;

12314


12315
        /* Product: '<S770>/Product1' */

12316
        _rtB->Product1_f[i_1] += _rtB->Product4[i_1] * _rtB->p_p[0];

12317


12318
        /* Product: '<S770>/Product5' */

12319
        _rtB->Sum_mx[i_1] += _rtB->Product4[i_1] * _rtB->Product2_p[0];

12320


12321
        /* Product: '<S770>/Product1' */

12322
        _rtB->Product1_f[i_1] += _rtB->Product4[i_1 + 3] * _rtB->p_p[1];

12323


12324
        /* Product: '<S770>/Product5' */

12325
        _rtB->Sum_mx[i_1] += _rtB->Product4[i_1 + 3] * _rtB->Product2_p[1];

12326


12327
        /* Product: '<S770>/Product1' */

12328
        _rtB->Product1_f[i_1] += _rtB->Product4[i_1 + 6] * _rtB->p_p[2];

12329


12330
        /* Product: '<S770>/Product5' */

12331
        _rtB->Sum_mx[i_1] += _rtB->Product4[i_1 + 6] * _rtB->Product2_p[2];

12332
      }

12333


12334
      /* Product: '<S770>/Product2' */

12335
      for (i_1 = 0; i_1 < 3; i_1++) {

12336
        _rtB->Product2_p[i_1] = 0.0;

12337
        _rtB->Product2_p[i_1] += _rtB->VectorConcatenate_nn[i_1] * 0.0;

12338
        _rtB->Product2_p[i_1] += _rtB->VectorConcatenate_nn[i_1 + 3] * 0.0;

12339
        _rtB->Product2_p[i_1] += _rtB->VectorConcatenate_nn[i_1 + 6] *

12340
          7.292115E-5;

12341
      }

12342


12343
      /* End of Product: '<S770>/Product2' */

12344
      for (i_1 = 0; i_1 < 3; i_1++) {

12345
        /* Product: '<S770>/Product3' */

12346
        _rtB->Product_d[i_1] = 0.0;

12347
        _rtB->Product_d[i_1] += _rtB->Product4[i_1] * _rtB->Product2_p[0];

12348
        _rtB->Product_d[i_1] += _rtB->Product4[i_1 + 3] * _rtB->Product2_p[1];

12349
        _rtB->Product_d[i_1] += _rtB->Product4[i_1 + 6] * _rtB->Product2_p[2];

12350


12351
        /* Gain: '<S770>/Gain' */

12352
        _rtB->Product_d[i_1] *= 2.0;

12353


12354
        /* Sum: '<S770>/Sum' */

12355
        _rtB->Sum_mx[i_1] += _rtB->Product_d[i_1];

12356
      }

12357


12358
      /* Product: '<S772>/Element product' */

12359
      _rtB->Sum1_h[0] = _rtB->Sum_mx[1] * _rtB->UnitConversion_o[2];

12360
      _rtB->Sum1_h[1] = _rtB->Sum_mx[2] * _rtB->UnitConversion_o[0];

12361
      _rtB->Sum1_h[2] = _rtB->Sum_mx[0] * _rtB->UnitConversion_o[1];

12362
      _rtB->Sum1_h[3] = _rtB->Sum_mx[2] * _rtB->UnitConversion_o[1];

12363
      _rtB->Sum1_h[4] = _rtB->Sum_mx[0] * _rtB->UnitConversion_o[2];

12364
      _rtB->Sum1_h[5] = _rtB->Sum_mx[1] * _rtB->UnitConversion_o[0];

12365


12366
      /* Sum: '<S772>/Add3' */

12367
      _rtB->Product_d[0] = _rtB->Sum1_h[0] - _rtB->Sum1_h[3];

12368


12369
      /* Sum: '<S770>/Sum1' */

12370
      _rtB->Product1_f[0] += _rtB->Product_d[0];

12371


12372
      /* Product: '<S471>/Product' */

12373
      _rtB->p_p[0] *= AFCS_MODEL1_rtC(S)->Product4_d;

12374


12375
      /* Sum: '<S772>/Add3' */

12376
      _rtB->Product_d[1] = _rtB->Sum1_h[1] - _rtB->Sum1_h[4];

12377


12378
      /* Sum: '<S770>/Sum1' */

12379
      _rtB->Product1_f[1] += _rtB->Product_d[1];

12380


12381
      /* Product: '<S471>/Product' */

12382
      _rtB->p_p[1] *= AFCS_MODEL1_rtC(S)->Product4_d;

12383


12384
      /* Sum: '<S772>/Add3' */

12385
      _rtB->Product_d[2] = _rtB->Sum1_h[2] - _rtB->Sum1_h[5];

12386


12387
      /* Sum: '<S770>/Sum1' */

12388
      _rtB->Product1_f[2] += _rtB->Product_d[2];

12389


12390
      /* Product: '<S471>/Product' */

12391
      _rtB->p_p[2] *= AFCS_MODEL1_rtC(S)->Product4_d;

12392


12393
      /* Saturate: '<S468>/Saturation' incorporates:

12394
       *  Saturate: '<S468>/Saturation1'

12395
       */

12396
      if (_rtB->Gain6_k > 1.0) {

12397
        _rtB->jxk = 1.0;

12398
        _rtB->Gain6_k = 1.0;

12399
      } else {

12400
        if (_rtB->Gain6_k < 0.0) {

12401
          _rtB->jxk = 0.0;

12402
        } else {

12403
          _rtB->jxk = _rtB->Gain6_k;

12404
        }

12405


12406
        if (_rtB->Gain6_k < 0.0) {

12407
          _rtB->Gain6_k = 0.0;

12408
        }

12409
      }

12410


12411
      /* End of Saturate: '<S468>/Saturation' */

12412


12413
      /* Lookup_n-D: '<S764>/eng_T' */

12414
      _rtB->jxk = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,

12415
        AFCS_MODEL1_ConstP.pooled24, AFCS_MODEL1_ConstP.pooled89, 1U);

12416


12417
      /* Product: '<S754>/Product3' */

12418
      _rtB->kxi = AFCS_MODEL1_rtC(S)->TrigonometricFunction1 * _rtB->jxk *

12419
        AFCS_MODEL1_rtC(S)->TrigonometricFunction2;

12420


12421
      /* Product: '<S754>/Product2' */

12422
      _rtB->ixj = _rtB->jxk * AFCS_MODEL1_rtC(S)->TrigonometricFunction;

12423


12424
      /* Product: '<S754>/Product4' */

12425
      _rtB->jxk = AFCS_MODEL1_rtC(S)->TrigonometricFunction1 * _rtB->jxk *

12426
        AFCS_MODEL1_rtC(S)->TrigonometricFunction3;

12427


12428
      /* Gain: '<S754>/Gain6' */

12429
      _rtB->jxk = -_rtB->jxk;

12430


12431
      /* Lookup_n-D: '<S758>/eng_T' */

12432
      _rtB->Gain6_k = AFCS_MODEL1_look1_binlxpw(_rtB->Gain6_k,

12433
        AFCS_MODEL1_ConstP.pooled24, AFCS_MODEL1_ConstP.pooled89, 1U);

12434


12435
      /* Product: '<S753>/Product3' */

12436
      _rtB->kxj = AFCS_MODEL1_rtC(S)->TrigonometricFunction1_h * _rtB->Gain6_k *

12437
        AFCS_MODEL1_rtC(S)->TrigonometricFunction2_b;

12438


12439
      /* Product: '<S753>/Product2' */

12440
      _rtB->ixk_a = _rtB->Gain6_k * AFCS_MODEL1_rtC(S)->TrigonometricFunction_d;

12441


12442
      /* Product: '<S753>/Product4' */

12443
      _rtB->Gain6_k = AFCS_MODEL1_rtC(S)->TrigonometricFunction1_h *

12444
        _rtB->Gain6_k * AFCS_MODEL1_rtC(S)->TrigonometricFunction3_d;

12445


12446
      /* Gain: '<S753>/Gain6' */

12447
      _rtB->Gain6_k = -_rtB->Gain6_k;

12448


12449
      /* Sum: '<S468>/Add' */

12450
      _rtB->Sum1_h[0] = _rtB->kxi + _rtB->kxj;

12451
      _rtB->Sum1_h[1] = _rtB->ixj + _rtB->ixk_a;

12452
      _rtB->Sum1_h[2] = _rtB->jxk + _rtB->Gain6_k;

12453
      for (i_1 = 0; i_1 < 3; i_1++) {

12454
        /* Product: '<S471>/Product1' */

12455
        _rtB->Product_d[i_1] = 0.0;

12456
        _rtB->Product_d[i_1] += _rtB->Product4[i_1] * _rtB->p_p[0];

12457
        _rtB->Product_d[i_1] += _rtB->Product4[i_1 + 3] * _rtB->p_p[1];

12458
        _rtB->Product_d[i_1] += _rtB->Product4[i_1 + 6] * _rtB->p_p[2];

12459


12460
        /* Sum: '<S468>/Add' */

12461
        _rtB->Sum1_h[i_1 + 3] = AFCS_MODEL1_rtC(S)->Sum3[i_1] + AFCS_MODEL1_rtC

12462
          (S)->Sum3_k[i_1];

12463
      }

12464


12465
      /* Product: '<S617>/Product13' incorporates:

12466
       *  Constant: '<S617>/Constant4'

12467
       */

12468
      _rtB->jxk = 0.002 * _rtB->UnitConversion_o[0];

12469


12470
      /* Gain: '<S617>/Gain' */

12471
      _rtB->jxk = -_rtB->jxk;

12472


12473
      /* Math: '<S617>/Math Function'

12474
       *

12475
       * About '<S617>/Math Function':

12476
       *  Operator: exp

12477
       */

12478
      _rtB->jxk = exp(_rtB->jxk);

12479


12480
      /* Product: '<S617>/Product12' incorporates:

12481
       *  Constant: '<S617>/Constant3'

12482
       */

12483
      _rtB->jxk *= 0.23;

12484


12485
      /* Product: '<S617>/Product14' */

12486
      _rtB->Product14 = _rtB->jxk * 0.0;

12487


12488
      /* Sum: '<S617>/Add' incorporates:

12489
       *  Constant: '<S617>/Constant1'

12490
       */

12491
      _rtB->kxi = _rtB->Product14 + 0.02;

12492


12493
      /* Switch: '<S617>/Switch' incorporates:

12494
       *  Constant: '<S617>/Constant8'

12495
       */

12496
      if (_rtB->UnitConversion_o[0] >= 0.1) {

12497
        _rtB->ixj = 1.0;

12498
      } else {

12499
        /* Product: '<S617>/Divide3' incorporates:

12500
         *  Constant: '<S617>/Constant9'

12501
         */

12502
        _rtB->Divide3 = _rtB->UnitConversion_o[0] / 0.1;

12503
        _rtB->ixj = _rtB->Divide3;

12504
      }

12505


12506
      /* End of Switch: '<S617>/Switch' */

12507


12508
      /* Product: '<S617>/Product5' */

12509
      _rtB->kxi *= _rtB->ixj;

12510


12511
      /* Saturate: '<S617>/Saturation4' */

12512
      if (_rtB->kxi > 0.7) {

12513
        _rtB->Saturation4 = 0.7;

12514
      } else if (_rtB->kxi < 0.0) {

12515
        _rtB->Saturation4 = 0.0;

12516
      } else {

12517
        _rtB->Saturation4 = _rtB->kxi;

12518
      }

12519


12520
      /* End of Saturate: '<S617>/Saturation4' */

12521


12522
      /* Math: '<S617>/Math Function7' */

12523
      _rtB->kxi = _rtB->Saturation4 * _rtB->Saturation4;

12524


12525
      /* Sum: '<S617>/Subtract1' */

12526
      _rtB->kxi = AFCS_MODEL1_rtC(S)->MathFunction6 - _rtB->kxi;

12527


12528
      /* Sqrt: '<S617>/Sqrt1' */

12529
      _rtB->kxi = sqrt(_rtB->kxi);

12530


12531
      /* SignalConversion: '<S628>/TmpSignal ConversionAtSinCosInport1' incorporates:

12532
       *  Constant: '<S617>/Constant13'

12533
       */

12534
      _rtB->Sum_mx[0] = _rtB->Merge_n[2];

12535
      _rtB->Sum_mx[1] = _rtB->Merge_n[1];

12536
      _rtB->Sum_mx[2] = 0.0;

12537


12538
      /* Trigonometry: '<S628>/SinCos' */

12539
      TrigonometricFunction4_o = cos(_rtB->Sum_mx[0]);

12540
      _rtB->Sum_mx[0] = sin(_rtB->Sum_mx[0]);

12541
      _rtB->p_p[0] = TrigonometricFunction4_o;

12542
      TrigonometricFunction4_o = cos(_rtB->Sum_mx[1]);

12543
      _rtB->Sum_mx[1] = sin(_rtB->Sum_mx[1]);

12544
      _rtB->p_p[1] = TrigonometricFunction4_o;

12545
      TrigonometricFunction4_o = cos(_rtB->Sum_mx[2]);

12546
      _rtB->Sum_mx[2] = sin(_rtB->Sum_mx[2]);

12547
      _rtB->p_p[2] = TrigonometricFunction4_o;

12548


12549
      /* Product: '<S641>/u(5)*u(6)' */

12550
      _rtB->VectorConcatenate_mr[0] = _rtB->p_p[1] * _rtB->p_p[2];

12551


12552
      /* Product: '<S644>/u(6)*u(1)*u(2)' */

12553
      _rtB->kxj = _rtB->p_p[2] * _rtB->Sum_mx[0] * _rtB->Sum_mx[1];

12554


12555
      /* Product: '<S644>/u(3)*u(4)' */

12556
      _rtB->ixk_a = _rtB->Sum_mx[2] * _rtB->p_p[0];

12557


12558
      /* Sum: '<S644>/Sum' */

12559
      _rtB->VectorConcatenate_mr[1] = _rtB->kxj - _rtB->ixk_a;

12560


12561
      /* Product: '<S647>/u(1)*u(3)' */

12562
      _rtB->kxj = _rtB->Sum_mx[0] * _rtB->Sum_mx[2];

12563


12564
      /* Product: '<S647>/u(2)*u(4)*u(6)' */

12565
      _rtB->ixk_a = _rtB->Sum_mx[1] * _rtB->p_p[0] * _rtB->p_p[2];

12566


12567
      /* Sum: '<S647>/Sum' */

12568
      _rtB->VectorConcatenate_mr[2] = _rtB->kxj + _rtB->ixk_a;

12569


12570
      /* Product: '<S642>/u(3)*u(5)' */

12571
      _rtB->VectorConcatenate_mr[3] = _rtB->Sum_mx[2] * _rtB->p_p[1];

12572


12573
      /* Product: '<S645>/u(1)*u(2)*u(3)' */

12574
      _rtB->kxj = _rtB->Sum_mx[0] * _rtB->Sum_mx[1] * _rtB->Sum_mx[2];

12575


12576
      /* Product: '<S645>/u(4)*u(6)' */

12577
      _rtB->ixk_a = _rtB->p_p[0] * _rtB->p_p[2];

12578


12579
      /* Sum: '<S645>/Sum' */

12580
      _rtB->VectorConcatenate_mr[4] = _rtB->kxj + _rtB->ixk_a;

12581


12582
      /* Product: '<S648>/u(2)*u(3)*u(4)' */

12583
      _rtB->kxj = _rtB->Sum_mx[1] * _rtB->Sum_mx[2] * _rtB->p_p[0];

12584


12585
      /* Product: '<S648>/u(1)*u(6)' */

12586
      _rtB->ixk_a = _rtB->p_p[2] * _rtB->Sum_mx[0];

12587


12588
      /* Sum: '<S648>/Sum' */

12589
      _rtB->VectorConcatenate_mr[5] = _rtB->kxj - _rtB->ixk_a;

12590


12591
      /* Gain: '<S643>/Gain2' */

12592
      _rtB->VectorConcatenate_mr[6] = -_rtB->Sum_mx[1];

12593


12594
      /* Product: '<S646>/u(1)*u(3)' */

12595
      _rtB->VectorConcatenate_mr[7] = _rtB->Sum_mx[0] * _rtB->p_p[1];

12596


12597
      /* Product: '<S649>/u(4)*u(5)' */

12598
      _rtB->VectorConcatenate_mr[8] = _rtB->p_p[0] * _rtB->p_p[1];

12599


12600
      /* Product: '<S635>/j x k' */

12601
      _rtB->kxj = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->LeftMain_c[2];

12602


12603
      /* Product: '<S635>/k x i' */

12604
      _rtB->ixk_a = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->LeftMain_c[0];

12605


12606
      /* Product: '<S635>/i x j' */

12607
      _rtB->jxi_p = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->LeftMain_c[1];

12608


12609
      /* Product: '<S636>/k x j' */

12610
      _rtB->ixk = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->LeftMain_c[1];

12611


12612
      /* Product: '<S636>/i x k' */

12613
      _rtB->jxi_k = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->LeftMain_c[2];

12614


12615
      /* Product: '<S636>/j x i' */

12616
      _rtB->jxi = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->LeftMain_c[0];

12617


12618
      /* Sum: '<S622>/Sum' */

12619
      _rtB->Sum_mx[0] = _rtB->kxj - _rtB->ixk;

12620
      _rtB->Sum_mx[1] = _rtB->ixk_a - _rtB->jxi_k;

12621
      _rtB->Sum_mx[2] = _rtB->jxi_p - _rtB->jxi;

12622
      for (i_1 = 0; i_1 < 3; i_1++) {

12623
        /* Math: '<S617>/Math Function1' */

12624
        _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_mr[i_1];

12625
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 3];

12626
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 6];

12627


12628
        /* Sum: '<S617>/Sum1' */

12629
        _rtB->Sum_mx[i_1] += _rtB->UnitConversion_o[i_1];

12630
      }

12631


12632
      /* Product: '<S617>/Product2' */

12633
      for (i_1 = 0; i_1 < 3; i_1++) {

12634
        _rtB->p_p[i_1] = 0.0;

12635
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->Sum_mx[0];

12636
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->Sum_mx[1];

12637
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->Sum_mx[2];

12638
      }

12639


12640
      /* End of Product: '<S617>/Product2' */

12641


12642
      /* Product: '<S632>/Product' */

12643
      _rtB->kxj = _rtB->p_p[0] * _rtB->p_p[0];

12644


12645
      /* Product: '<S632>/Product1' */

12646
      _rtB->ixk_a = _rtB->p_p[1] * _rtB->p_p[1];

12647


12648
      /* Product: '<S632>/Product2' */

12649
      _rtB->jxi_p = _rtB->p_p[2] * _rtB->p_p[2];

12650


12651
      /* Sum: '<S632>/Sum' */

12652
      _rtB->kxj = (_rtB->kxj + _rtB->ixk_a) + _rtB->jxi_p;

12653


12654
      /* Math: '<S617>/Airspeed'

12655
       *

12656
       * About '<S617>/Airspeed':

12657
       *  Operator: sqrt

12658
       */

12659
      if (_rtB->kxj < 0.0) {

12660
        _rtB->kxj = -sqrt(fabs(_rtB->kxj));

12661
      } else {

12662
        _rtB->kxj = sqrt(_rtB->kxj);

12663
      }

12664


12665
      /* End of Math: '<S617>/Airspeed' */

12666


12667
      /* Saturate: '<S617>/Saturation' */

12668
      if (_rtB->kxj <= 0.01) {

12669
        _rtB->kxj = 0.01;

12670
      }

12671


12672
      /* End of Saturate: '<S617>/Saturation' */

12673


12674
      /* Product: '<S617>/Divide' */

12675
      _rtB->kxj = _rtB->p_p[1] / _rtB->kxj;

12676


12677
      /* Trigonometry: '<S617>/Trigonometric Function' */

12678
      if (_rtB->kxj > 1.0) {

12679
        TrigonometricFunction4_o = 1.0;

12680
      } else if (_rtB->kxj < -1.0) {

12681
        TrigonometricFunction4_o = -1.0;

12682
      } else {

12683
        TrigonometricFunction4_o = _rtB->kxj;

12684
      }

12685


12686
      _rtB->kxj = asin(TrigonometricFunction4_o);

12687


12688
      /* End of Trigonometry: '<S617>/Trigonometric Function' */

12689


12690
      /* Gain: '<S625>/Unit Conversion' */

12691
      _rtB->UnitConversion_h = 57.295779513082323 * _rtB->kxj;

12692


12693
      /* Abs: '<S617>/Abs1' */

12694
      _rtB->kxj = fabs(_rtB->UnitConversion_h);

12695


12696
      /* Saturate: '<S617>/Saturation7' */

12697
      if (_rtB->kxj > 10.0) {

12698
        _rtB->kxj = 10.0;

12699
      } else {

12700
        if (_rtB->kxj < 0.0) {

12701
          _rtB->kxj = 0.0;

12702
        }

12703
      }

12704


12705
      /* End of Saturate: '<S617>/Saturation7' */

12706


12707
      /* Fcn: '<S617>/Fcn1' */

12708
      _rtB->kxj = 0.08 * _rtB->kxj - 0.003 * AFCS_MODEL1_rt_powd_snf(_rtB->kxj,

12709
        2.0);

12710


12711
      /* Product: '<S617>/Product18' */

12712
      _rtB->Product18 = _rtB->kxj * _rtB->ixj;

12713


12714
      /* RelationalOperator: '<S630>/LowerRelop1' */

12715
      _rtB->LowerRelop1 = (_rtB->Product18 > _rtB->kxi);

12716


12717
      /* Switch: '<S630>/Switch2' */

12718
      if (!_rtB->LowerRelop1) {

12719
        /* RelationalOperator: '<S630>/UpperRelop' incorporates:

12720
         *  Constant: '<S617>/Constant11'

12721
         */

12722
        _rtB->UpperRelop_k = (_rtB->Product18 < 0.0);

12723


12724
        /* Switch: '<S630>/Switch' incorporates:

12725
         *  Constant: '<S617>/Constant11'

12726
         */

12727
        if (_rtB->UpperRelop_k) {

12728
          _rtB->Switch_a = 0.0;

12729
        } else {

12730
          _rtB->Switch_a = _rtB->Product18;

12731
        }

12732


12733
        /* End of Switch: '<S630>/Switch' */

12734
        _rtB->kxi = _rtB->Switch_a;

12735
      }

12736


12737
      /* End of Switch: '<S630>/Switch2' */

12738


12739
      /* Signum: '<S617>/Sign1' */

12740
      if (_rtB->UnitConversion_h < 0.0) {

12741
        _rtB->kxj = -1.0;

12742
      } else if (_rtB->UnitConversion_h > 0.0) {

12743
        _rtB->kxj = 1.0;

12744
      } else if (_rtB->UnitConversion_h == 0.0) {

12745
        _rtB->kxj = 0.0;

12746
      } else {

12747
        _rtB->kxj = _rtB->UnitConversion_h;

12748
      }

12749


12750
      /* End of Signum: '<S617>/Sign1' */

12751


12752
      /* Product: '<S617>/Product19' */

12753
      _rtB->Product19 = _rtB->kxi * _rtB->kxj;

12754


12755
      /* Sum: '<S617>/Sum4' */

12756
      _rtB->kxi = _rtB->Product19;

12757


12758
      /* Gain: '<S617>/Gain2' */

12759
      _rtB->Gain2 = _rtB->kxi;

12760
    }

12761


12762
    if (ssIsSampleHit(S, 1, 0)) {

12763
      /* Gain: '<S466>/zground' */

12764
      _rtB->zground = -_rtB->DataTypeConversion1;

12765
    }

12766


12767
    /* Outputs for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

12768
    AFCS_MODEL1_LMainGearGeometryForcesandMoments(S, _rtB->zground, _rtB->Alt,

12769
      _rtB->Merge_n[2], _rtB->Merge_n[1], AFCS_MODEL1_rtC(S)->LeftMain,

12770
      _rtB->Saturation4, _rtB->Gain2, &_rtB->LMainGearGeometryForcesandMomen,

12771
      (ConstB_LMainGearGeometryForce_T *)&AFCS_MODEL1_rtC(S)

12772
      ->LMainGearGeometryForcesandMomen);

12773


12774
    /* End of Outputs for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

12775
    if (1) {

12776
      /* Product: '<S617>/Product15' */

12777
      _rtB->kxi = 0.0 * _rtB->jxk;

12778


12779
      /* Sum: '<S617>/Add1' incorporates:

12780
       *  Constant: '<S617>/Constant2'

12781
       */

12782
      _rtB->kxi += 0.02;

12783


12784
      /* Product: '<S617>/Product6' */

12785
      _rtB->kxi *= _rtB->ixj;

12786


12787
      /* Saturate: '<S617>/Saturation5' */

12788
      if (_rtB->kxi > 0.7) {

12789
        _rtB->Saturation5 = 0.7;

12790
      } else if (_rtB->kxi < 0.0) {

12791
        _rtB->Saturation5 = 0.0;

12792
      } else {

12793
        _rtB->Saturation5 = _rtB->kxi;

12794
      }

12795


12796
      /* End of Saturate: '<S617>/Saturation5' */

12797


12798
      /* Math: '<S617>/Math Function5' */

12799
      _rtB->jxk = _rtB->Saturation5 * _rtB->Saturation5;

12800


12801
      /* Sum: '<S617>/Subtract' */

12802
      _rtB->jxk = AFCS_MODEL1_rtC(S)->MathFunction4 - _rtB->jxk;

12803


12804
      /* Sqrt: '<S617>/Sqrt' */

12805
      _rtB->jxk = sqrt(_rtB->jxk);

12806


12807
      /* Product: '<S639>/j x k' */

12808
      _rtB->kxi = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->RightMain_k[2];

12809


12810
      /* Product: '<S639>/k x i' */

12811
      _rtB->kxj = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->RightMain_k[0];

12812


12813
      /* Product: '<S639>/i x j' */

12814
      _rtB->ixk_a = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->RightMain_k[1];

12815


12816
      /* Product: '<S640>/k x j' */

12817
      _rtB->jxi_p = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->RightMain_k[1];

12818


12819
      /* Product: '<S640>/i x k' */

12820
      _rtB->ixk = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->RightMain_k[2];

12821


12822
      /* Product: '<S640>/j x i' */

12823
      _rtB->jxi_k = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->RightMain_k[0];

12824


12825
      /* Sum: '<S624>/Sum' */

12826
      _rtB->Sum_mx[0] = _rtB->kxi - _rtB->jxi_p;

12827
      _rtB->Sum_mx[1] = _rtB->kxj - _rtB->ixk;

12828
      _rtB->Sum_mx[2] = _rtB->ixk_a - _rtB->jxi_k;

12829
      for (i_1 = 0; i_1 < 3; i_1++) {

12830
        /* Math: '<S617>/Math Function2' */

12831
        _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_mr[i_1];

12832
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 3];

12833
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 6];

12834


12835
        /* Sum: '<S617>/Sum2' */

12836
        _rtB->Sum_mx[i_1] += _rtB->UnitConversion_o[i_1];

12837
      }

12838


12839
      /* Product: '<S617>/Product4' */

12840
      for (i_1 = 0; i_1 < 3; i_1++) {

12841
        _rtB->p_p[i_1] = 0.0;

12842
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->Sum_mx[0];

12843
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->Sum_mx[1];

12844
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->Sum_mx[2];

12845
      }

12846


12847
      /* End of Product: '<S617>/Product4' */

12848


12849
      /* Product: '<S633>/Product' */

12850
      _rtB->kxi = _rtB->p_p[0] * _rtB->p_p[0];

12851


12852
      /* Product: '<S633>/Product1' */

12853
      _rtB->kxj = _rtB->p_p[1] * _rtB->p_p[1];

12854


12855
      /* Product: '<S633>/Product2' */

12856
      _rtB->ixk_a = _rtB->p_p[2] * _rtB->p_p[2];

12857


12858
      /* Sum: '<S633>/Sum' */

12859
      _rtB->kxi = (_rtB->kxi + _rtB->kxj) + _rtB->ixk_a;

12860


12861
      /* Math: '<S617>/Airspeed1'

12862
       *

12863
       * About '<S617>/Airspeed1':

12864
       *  Operator: sqrt

12865
       */

12866
      if (_rtB->kxi < 0.0) {

12867
        _rtB->kxi = -sqrt(fabs(_rtB->kxi));

12868
      } else {

12869
        _rtB->kxi = sqrt(_rtB->kxi);

12870
      }

12871


12872
      /* End of Math: '<S617>/Airspeed1' */

12873


12874
      /* Saturate: '<S617>/Saturation1' */

12875
      if (_rtB->kxi <= 0.01) {

12876
        _rtB->kxi = 0.01;

12877
      }

12878


12879
      /* End of Saturate: '<S617>/Saturation1' */

12880


12881
      /* Product: '<S617>/Divide1' */

12882
      _rtB->kxi = _rtB->p_p[1] / _rtB->kxi;

12883


12884
      /* Trigonometry: '<S617>/Trigonometric Function1' */

12885
      if (_rtB->kxi > 1.0) {

12886
        TrigonometricFunction4_o = 1.0;

12887
      } else if (_rtB->kxi < -1.0) {

12888
        TrigonometricFunction4_o = -1.0;

12889
      } else {

12890
        TrigonometricFunction4_o = _rtB->kxi;

12891
      }

12892


12893
      _rtB->kxi = asin(TrigonometricFunction4_o);

12894


12895
      /* End of Trigonometry: '<S617>/Trigonometric Function1' */

12896


12897
      /* Gain: '<S626>/Unit Conversion' */

12898
      _rtB->UnitConversion_c = 57.295779513082323 * _rtB->kxi;

12899


12900
      /* Abs: '<S617>/Abs2' */

12901
      _rtB->kxi = fabs(_rtB->UnitConversion_c);

12902


12903
      /* Saturate: '<S617>/Saturation8' */

12904
      if (_rtB->kxi > 10.0) {

12905
        _rtB->kxi = 10.0;

12906
      } else {

12907
        if (_rtB->kxi < 0.0) {

12908
          _rtB->kxi = 0.0;

12909
        }

12910
      }

12911


12912
      /* End of Saturate: '<S617>/Saturation8' */

12913


12914
      /* Fcn: '<S617>/Fcn2' */

12915
      _rtB->kxi = 0.08 * _rtB->kxi - 0.003 * AFCS_MODEL1_rt_powd_snf(_rtB->kxi,

12916
        2.0);

12917


12918
      /* Product: '<S617>/Product20' */

12919
      _rtB->Product20 = _rtB->kxi * _rtB->ixj;

12920


12921
      /* RelationalOperator: '<S629>/LowerRelop1' */

12922
      _rtB->LowerRelop1 = (_rtB->Product20 > _rtB->jxk);

12923


12924
      /* Switch: '<S629>/Switch2' */

12925
      if (!_rtB->LowerRelop1) {

12926
        /* RelationalOperator: '<S629>/UpperRelop' incorporates:

12927
         *  Constant: '<S617>/Constant7'

12928
         */

12929
        _rtB->UpperRelop_p = (_rtB->Product20 < 0.0);

12930


12931
        /* Switch: '<S629>/Switch' incorporates:

12932
         *  Constant: '<S617>/Constant7'

12933
         */

12934
        if (_rtB->UpperRelop_p) {

12935
          _rtB->Switch_g = 0.0;

12936
        } else {

12937
          _rtB->Switch_g = _rtB->Product20;

12938
        }

12939


12940
        /* End of Switch: '<S629>/Switch' */

12941
        _rtB->jxk = _rtB->Switch_g;

12942
      }

12943


12944
      /* End of Switch: '<S629>/Switch2' */

12945


12946
      /* Signum: '<S617>/Sign2' */

12947
      if (_rtB->UnitConversion_c < 0.0) {

12948
        _rtB->kxi = -1.0;

12949
      } else if (_rtB->UnitConversion_c > 0.0) {

12950
        _rtB->kxi = 1.0;

12951
      } else if (_rtB->UnitConversion_c == 0.0) {

12952
        _rtB->kxi = 0.0;

12953
      } else {

12954
        _rtB->kxi = _rtB->UnitConversion_c;

12955
      }

12956


12957
      /* End of Signum: '<S617>/Sign2' */

12958


12959
      /* Product: '<S617>/Product21' */

12960
      _rtB->Product21 = _rtB->jxk * _rtB->kxi;

12961


12962
      /* Sum: '<S617>/Sum5' */

12963
      _rtB->jxk = _rtB->Product21;

12964


12965
      /* Gain: '<S617>/Gain3' */

12966
      _rtB->Gain3 = _rtB->jxk;

12967
    }

12968


12969
    /* Outputs for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

12970
    AFCS_MODEL1_RMainGearGeometryForcesandMoments(S, _rtB->zground, _rtB->Alt,

12971
      _rtB->Merge_n[2], _rtB->Merge_n[1], AFCS_MODEL1_rtC(S)->RightMain,

12972
      _rtB->Saturation5, _rtB->Gain3, _rtB->Merge_n,

12973
      &_rtB->RMainGearGeometryForcesandMomen, (ConstB_RMainGearGeometryForce_T *)

12974
      &AFCS_MODEL1_rtC(S)->RMainGearGeometryForcesandMomen);

12975


12976
    /* End of Outputs for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

12977
    if (ssIsSampleHit(S, 1, 0)) {

12978
      /* UnitDelay: '<S682>/Unit Delay' */

12979
      _rtB->Sum1_fe = ((int8_T *)ssGetDWork(S, 371))[0];

12980


12981
      /* Sum: '<S682>/Sum' incorporates:

12982
       *  Constant: '<S446>/Constant2'

12983
       */

12984
      _rtB->Sum1_fe = (int8_T)(1 - _rtB->Sum1_fe);

12985


12986
      /* RelationalOperator: '<S684>/Compare' incorporates:

12987
       *  Constant: '<S684>/Constant'

12988
       */

12989
      _rtB->Compare = (uint8_T)(_rtB->Sum1_fe > 0);

12990


12991
      /* Logic: '<S682>/Logical Operator' */

12992
      _rtB->LogicalOperator_m = (_rtB->Compare != 0);

12993


12994
      /* UnitDelay: '<S683>/Unit Delay' */

12995
      _rtB->Sum1_fe = ((int8_T *)ssGetDWork(S, 372))[0];

12996


12997
      /* Sum: '<S683>/Sum1' incorporates:

12998
       *  Constant: '<S446>/Constant2'

12999
       */

13000
      _rtB->Sum1_fe = (int8_T)(1 - _rtB->Sum1_fe);

13001


13002
      /* RelationalOperator: '<S685>/Compare' incorporates:

13003
       *  Constant: '<S685>/Constant'

13004
       */

13005
      _rtB->Compare_i = (_rtB->Sum1_fe < 0);

13006


13007
      /* UnitDelay: '<S619>/Unit Delay1' */

13008
      _rtB->UnitDelay1 = ((real_T *)ssGetDWork(S, 0))[0];

13009
    }

13010


13011
    if (1) {

13012
      /* Product: '<S617>/Product3' incorporates:

13013
       *  Constant: '<S617>/Constant'

13014
       */

13015
      _rtB->jxk = 0.02 * _rtB->ixj;

13016


13017
      /* Saturate: '<S617>/Saturation3' */

13018
      if (_rtB->jxk > 0.7) {

13019
        _rtB->Saturation3 = 0.7;

13020
      } else if (_rtB->jxk < 0.0) {

13021
        _rtB->Saturation3 = 0.0;

13022
      } else {

13023
        _rtB->Saturation3 = _rtB->jxk;

13024
      }

13025


13026
      /* End of Saturate: '<S617>/Saturation3' */

13027


13028
      /* Math: '<S617>/Math Function9' */

13029
      _rtB->jxk = _rtB->Saturation3 * _rtB->Saturation3;

13030


13031
      /* Sum: '<S617>/Subtract2' */

13032
      _rtB->jxk = AFCS_MODEL1_rtC(S)->MathFunction8 - _rtB->jxk;

13033


13034
      /* Product: '<S637>/j x k' */

13035
      _rtB->kxi = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->Nose_d[2];

13036


13037
      /* Product: '<S637>/k x i' */

13038
      _rtB->kxj = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->Nose_d[0];

13039


13040
      /* Product: '<S637>/i x j' */

13041
      _rtB->ixk_a = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->Nose_d[1];

13042


13043
      /* Product: '<S638>/k x j' */

13044
      _rtB->jxi_p = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->Nose_d[1];

13045


13046
      /* Product: '<S638>/i x k' */

13047
      _rtB->ixk = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->Nose_d[2];

13048


13049
      /* Product: '<S638>/j x i' */

13050
      _rtB->jxi_k = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->Nose_d[0];

13051


13052
      /* Sum: '<S623>/Sum' */

13053
      _rtB->Sum_mx[0] = _rtB->kxi - _rtB->jxi_p;

13054
      _rtB->Sum_mx[1] = _rtB->kxj - _rtB->ixk;

13055
      _rtB->Sum_mx[2] = _rtB->ixk_a - _rtB->jxi_k;

13056
      for (i_1 = 0; i_1 < 3; i_1++) {

13057
        /* Math: '<S617>/Math Function3' */

13058
        _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_mr[i_1];

13059
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 3];

13060
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 6];

13061


13062
        /* Sum: '<S617>/Sum' */

13063
        _rtB->Sum_mx[i_1] += _rtB->UnitConversion_o[i_1];

13064
      }

13065


13066
      /* Product: '<S617>/Product1' */

13067
      for (i_1 = 0; i_1 < 3; i_1++) {

13068
        _rtB->p_p[i_1] = 0.0;

13069
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->Sum_mx[0];

13070
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->Sum_mx[1];

13071
        _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->Sum_mx[2];

13072
      }

13073


13074
      /* End of Product: '<S617>/Product1' */

13075


13076
      /* Product: '<S634>/Product' */

13077
      _rtB->kxi = _rtB->p_p[0] * _rtB->p_p[0];

13078


13079
      /* Product: '<S634>/Product1' */

13080
      _rtB->kxj = _rtB->p_p[1] * _rtB->p_p[1];

13081


13082
      /* Product: '<S634>/Product2' */

13083
      _rtB->ixk_a = _rtB->p_p[2] * _rtB->p_p[2];

13084


13085
      /* Sum: '<S634>/Sum' */

13086
      _rtB->kxi = (_rtB->kxi + _rtB->kxj) + _rtB->ixk_a;

13087


13088
      /* Math: '<S617>/Airspeed2'

13089
       *

13090
       * About '<S617>/Airspeed2':

13091
       *  Operator: sqrt

13092
       */

13093
      if (_rtB->kxi < 0.0) {

13094
        _rtB->kxi = -sqrt(fabs(_rtB->kxi));

13095
      } else {

13096
        _rtB->kxi = sqrt(_rtB->kxi);

13097
      }

13098


13099
      /* End of Math: '<S617>/Airspeed2' */

13100


13101
      /* Saturate: '<S617>/Saturation2' */

13102
      if (_rtB->kxi <= 0.01) {

13103
        _rtB->kxi = 0.01;

13104
      }

13105


13106
      /* End of Saturate: '<S617>/Saturation2' */

13107


13108
      /* Product: '<S617>/Divide2' */

13109
      _rtB->kxi = _rtB->p_p[1] / _rtB->kxi;

13110


13111
      /* Trigonometry: '<S617>/Trigonometric Function2' */

13112
      if (_rtB->kxi > 1.0) {

13113
        TrigonometricFunction4_o = 1.0;

13114
      } else if (_rtB->kxi < -1.0) {

13115
        TrigonometricFunction4_o = -1.0;

13116
      } else {

13117
        TrigonometricFunction4_o = _rtB->kxi;

13118
      }

13119


13120
      _rtB->kxi = asin(TrigonometricFunction4_o);

13121


13122
      /* End of Trigonometry: '<S617>/Trigonometric Function2' */

13123


13124
      /* Gain: '<S627>/Unit Conversion' */

13125
      _rtB->UnitConversion_oi = 57.295779513082323 * _rtB->kxi;

13126


13127
      /* Integrator: '<S683>/Integrator' */

13128
      /* Limited  Integrator  */

13129
      if (ssIsMajorTimeStep(S)) {

13130
        resetIntg = false;

13131
        didZcEventOccur = (_rtB->Compare_i && (_rtZCE->Integrator_Reset_ZCE !=

13132
          POS_ZCSIG));

13133
        _rtZCE->Integrator_Reset_ZCE = _rtB->Compare_i;

13134


13135
        /* evaluate zero-crossings */

13136
        if (didZcEventOccur || (((int_T *)ssGetDWork(S, 348))[0] != 0)) {

13137
          resetIntg = true;

13138
          _rtX->Integrator_CSTATE = _rtB->UnitDelay1;

13139
        }

13140


13141
        if (resetIntg) {

13142
          ssSetSolverNeedsReset(S);

13143
        }

13144
      }

13145


13146
      if (_rtX->Integrator_CSTATE >= 8.0) {

13147
        if (_rtX->Integrator_CSTATE != 8.0) {

13148
          _rtX->Integrator_CSTATE = 8.0;

13149
          ssSetSolverNeedsReset(S);

13150
        }

13151
      } else {

13152
        if ((_rtX->Integrator_CSTATE <= -8.0) && (_rtX->Integrator_CSTATE !=

13153
             -8.0)) {

13154
          _rtX->Integrator_CSTATE = -8.0;

13155
          ssSetSolverNeedsReset(S);

13156
        }

13157
      }

13158


13159
      _rtB->Integrator = _rtX->Integrator_CSTATE;

13160


13161
      /* End of Integrator: '<S683>/Integrator' */

13162


13163
      /* Integrator: '<S682>/Integrator' */

13164
      /* Limited  Integrator  */

13165
      if (ssIsMajorTimeStep(S)) {

13166
        resetIntg = false;

13167
        didZcEventOccur = (_rtB->LogicalOperator_m &&

13168
                           (_rtZCE->Integrator_Reset_ZCE_f != POS_ZCSIG));

13169
        _rtZCE->Integrator_Reset_ZCE_f = _rtB->LogicalOperator_m;

13170


13171
        /* evaluate zero-crossings */

13172
        if (didZcEventOccur || (((int_T *)ssGetDWork(S, 349))[0] != 0)) {

13173
          resetIntg = true;

13174
          _rtX->Integrator_CSTATE_l = _rtB->Integrator;

13175
        }

13176


13177
        if (resetIntg) {

13178
          ssSetSolverNeedsReset(S);

13179
        }

13180
      }

13181


13182
      if (_rtX->Integrator_CSTATE_l >= 30.0) {

13183
        if (_rtX->Integrator_CSTATE_l != 30.0) {

13184
          _rtX->Integrator_CSTATE_l = 30.0;

13185
          ssSetSolverNeedsReset(S);

13186
        }

13187
      } else {

13188
        if ((_rtX->Integrator_CSTATE_l <= -30.0) && (_rtX->Integrator_CSTATE_l

13189
             != -30.0)) {

13190
          _rtX->Integrator_CSTATE_l = -30.0;

13191
          ssSetSolverNeedsReset(S);

13192
        }

13193
      }

13194


13195
      _rtB->Integrator_m = _rtX->Integrator_CSTATE_l;

13196


13197
      /* End of Integrator: '<S682>/Integrator' */

13198


13199
      /* Sum: '<S617>/Sum3' */

13200
      _rtB->Sum3 = _rtB->UnitConversion_oi - _rtB->Integrator_m;

13201


13202
      /* Abs: '<S617>/Abs' */

13203
      _rtB->kxi = fabs(_rtB->Sum3);

13204


13205
      /* Saturate: '<S617>/Saturation6' */

13206
      if (_rtB->kxi > 10.0) {

13207
        _rtB->kxi = 10.0;

13208
      } else {

13209
        if (_rtB->kxi < 0.0) {

13210
          _rtB->kxi = 0.0;

13211
        }

13212
      }

13213


13214
      /* End of Saturate: '<S617>/Saturation6' */

13215


13216
      /* Fcn: '<S617>/Fcn' */

13217
      _rtB->kxi = 0.08 * _rtB->kxi - 0.003 * AFCS_MODEL1_rt_powd_snf(_rtB->kxi,

13218
        2.0);

13219


13220
      /* Product: '<S617>/Product16' */

13221
      _rtB->Product16 = _rtB->kxi * _rtB->ixj;

13222


13223
      /* RelationalOperator: '<S631>/LowerRelop1' */

13224
      _rtB->LowerRelop1 = (_rtB->Product16 > _rtB->jxk);

13225


13226
      /* Switch: '<S631>/Switch2' */

13227
      if (!_rtB->LowerRelop1) {

13228
        /* RelationalOperator: '<S631>/UpperRelop' incorporates:

13229
         *  Constant: '<S617>/Constant12'

13230
         */

13231
        _rtB->UpperRelop = (_rtB->Product16 < 0.0);

13232


13233
        /* Switch: '<S631>/Switch' incorporates:

13234
         *  Constant: '<S617>/Constant12'

13235
         */

13236
        if (_rtB->UpperRelop) {

13237
          _rtB->Switch_o = 0.0;

13238
        } else {

13239
          _rtB->Switch_o = _rtB->Product16;

13240
        }

13241


13242
        /* End of Switch: '<S631>/Switch' */

13243
        _rtB->jxk = _rtB->Switch_o;

13244
      }

13245


13246
      /* End of Switch: '<S631>/Switch2' */

13247


13248
      /* Signum: '<S617>/Sign' */

13249
      if (_rtB->Sum3 < 0.0) {

13250
        _rtB->kxi = -1.0;

13251
      } else if (_rtB->Sum3 > 0.0) {

13252
        _rtB->kxi = 1.0;

13253
      } else if (_rtB->Sum3 == 0.0) {

13254
        _rtB->kxi = 0.0;

13255
      } else {

13256
        _rtB->kxi = _rtB->Sum3;

13257
      }

13258


13259
      /* End of Signum: '<S617>/Sign' */

13260


13261
      /* Product: '<S617>/Product17' */

13262
      _rtB->jxk *= _rtB->kxi;

13263


13264
      /* Gain: '<S617>/Gain1' */

13265
      _rtB->Gain1 = _rtB->jxk;

13266
    }

13267


13268
    /* Outputs for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

13269
    AFCS_MODEL1_NoseGearGeometryForcesandMoments(S, _rtB->zground, _rtB->Alt,

13270
      _rtB->Merge_n[2], _rtB->Merge_n[1], AFCS_MODEL1_rtC(S)->Nose,

13271
      _rtB->Saturation3, _rtB->Gain1, _rtB->Merge_n,

13272
      &_rtB->NoseGearGeometryForcesandMoment, (ConstB_NoseGearGeometryForces_T *)

13273
      &AFCS_MODEL1_rtC(S)->NoseGearGeometryForcesandMoment);

13274


13275
    /* End of Outputs for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

13276
    if (1) {

13277
      /* Sum: '<S466>/Sum2' */

13278
      _rtB->Sum2_g[0] =

13279
        (_rtB->LMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[0] +

13280
         _rtB->RMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[0]) +

13281
        _rtB->NoseGearGeometryForcesandMoment.LocalForcestoBodyAxes1[0];

13282
      _rtB->Sum2_g[3] = (_rtB->LMainGearGeometryForcesandMomen.Gain2[0] +

13283
                         _rtB->RMainGearGeometryForcesandMomen.Gain2[0]) +

13284
        _rtB->NoseGearGeometryForcesandMoment.Gain1[0];

13285


13286
      /* Sum: '<S446>/Sum1' */

13287
      tmp_0[0] = _rtB->Product2_f[0];

13288
      tmp_0[3] = _rtB->Sum1[0];

13289


13290
      /* Sum: '<S466>/Sum2' */

13291
      _rtB->Sum2_g[1] =

13292
        (_rtB->LMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[1] +

13293
         _rtB->RMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[1]) +

13294
        _rtB->NoseGearGeometryForcesandMoment.LocalForcestoBodyAxes1[1];

13295
      _rtB->Sum2_g[4] = (_rtB->LMainGearGeometryForcesandMomen.Gain2[1] +

13296
                         _rtB->RMainGearGeometryForcesandMomen.Gain2[1]) +

13297
        _rtB->NoseGearGeometryForcesandMoment.Gain1[1];

13298


13299
      /* Sum: '<S446>/Sum1' */

13300
      tmp_0[1] = _rtB->Product2_f[1];

13301
      tmp_0[4] = _rtB->Sum1[1];

13302


13303
      /* Sum: '<S466>/Sum2' */

13304
      _rtB->Sum2_g[2] =

13305
        (_rtB->LMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[2] +

13306
         _rtB->RMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[2]) +

13307
        _rtB->NoseGearGeometryForcesandMoment.LocalForcestoBodyAxes1[2];

13308
      _rtB->Sum2_g[5] = (_rtB->LMainGearGeometryForcesandMomen.Gain2[2] +

13309
                         _rtB->RMainGearGeometryForcesandMomen.Gain2[2]) +

13310
        _rtB->NoseGearGeometryForcesandMoment.Gain1[2];

13311


13312
      /* Sum: '<S446>/Sum1' */

13313
      tmp_0[2] = _rtB->Product2_f[2];

13314
      tmp_0[5] = _rtB->Sum1[2];

13315
      for (i_1 = 0; i_1 < 6; i_1++) {

13316
        _rtB->Sum1_h[i_1] = (tmp_0[i_1] + _rtB->Sum1_h[i_1]) + _rtB->Sum2_g[i_1];

13317
      }

13318


13319
      /* Sum: '<S446>/Sum' */

13320
      _rtB->Product_d[0] += _rtB->Sum1_h[0];

13321


13322
      /* Product: '<S470>/Product' */

13323
      _rtB->Product_d[0] /= AFCS_MODEL1_rtC(S)->Product4_d;

13324


13325
      /* Sum: '<S469>/Sum6' */

13326
      _rtB->Product1_f[0] = _rtB->Product_d[0] - _rtB->Product1_f[0];

13327


13328
      /* Sum: '<S446>/Sum' */

13329
      _rtB->Product_d[1] += _rtB->Sum1_h[1];

13330


13331
      /* Product: '<S470>/Product' */

13332
      _rtB->Product_d[1] /= AFCS_MODEL1_rtC(S)->Product4_d;

13333


13334
      /* Sum: '<S469>/Sum6' */

13335
      _rtB->Product1_f[1] = _rtB->Product_d[1] - _rtB->Product1_f[1];

13336


13337
      /* Sum: '<S446>/Sum' */

13338
      _rtB->Product_d[2] += _rtB->Sum1_h[2];

13339


13340
      /* Product: '<S470>/Product' */

13341
      _rtB->Product_d[2] /= AFCS_MODEL1_rtC(S)->Product4_d;

13342


13343
      /* Sum: '<S469>/Sum6' */

13344
      _rtB->Product1_f[2] = _rtB->Product_d[2] - _rtB->Product1_f[2];

13345


13346
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

13347
      _rtB->Abx = _rtB->Product1_f[0];

13348


13349
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

13350
      _rtB->Aby = _rtB->Product1_f[1];

13351


13352
      /* Gain: '<S469>/Gain1' */

13353
      _rtB->jxk = -_rtB->Product1_f[2];

13354


13355
      /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */

13356
      _rtB->Abz = _rtB->jxk;

13357


13358
      /* Product: '<S613>/Product' */

13359
      _rtB->jxk = _rtB->Sum_m[0] * _rtB->Sum_m[0];

13360


13361
      /* Product: '<S613>/Product1' */

13362
      _rtB->kxi = _rtB->Sum_m[1] * _rtB->Sum_m[1];

13363


13364
      /* Product: '<S613>/Product2' */

13365
      _rtB->ixj = _rtB->Sum_m[2] * _rtB->Sum_m[2];

13366


13367
      /* Sum: '<S613>/Sum' */

13368
      _rtB->jxk = (_rtB->jxk + _rtB->kxi) + _rtB->ixj;

13369


13370
      /* Sqrt: '<S606>/vt' */

13371
      _rtB->jxk = sqrt(_rtB->jxk);

13372


13373
      /* Product: '<S606>/Product1' */

13374
      _rtB->Product1 = _rtB->jxk / _rtB->sound;

13375


13376
      /* MATLAB Function: '<S464>/Embedded MATLAB Function' */

13377
      /* MATLAB Function 'AFCS_MODEL1/Subsystem5/Subsystem3/Devond/Embedded MATLAB Function': '<S603>:1' */

13378
      /*  This block supports an embeddable subset of the MATLAB language. */

13379
      /*  See the help menu for details.  */

13380
      /* '<S603>:1:4' */

13381
      if (_rtB->Product1 < 1.0) {

13382
        /* '<S603>:1:5' */

13383
        /* '<S603>:1:6' */

13384
        TrigonometricFunction4_o = AFCS_MODEL1_rt_powd_snf(_rtB->Product1 *

13385
          _rtB->Product1 * 0.19999999999999996 + 1.0, 3.5);

13386
      } else {

13387
        /* '<S603>:1:8' */

13388
        TrigonometricFunction4_o = AFCS_MODEL1_rt_powd_snf(_rtB->Product1 *

13389
          _rtB->Product1 * 2.4 / 2.0, 3.5000000000000004) /

13390
          AFCS_MODEL1_rt_powd_snf((_rtB->Product1 * _rtB->Product1 *

13391
          1.1666666666666667 - 0.28571428571428564) + 1.0, 2.5000000000000004);

13392
      }

13393


13394
      /* '<S603>:1:10' */

13395
      TrigonometricFunction4_o = (TrigonometricFunction4_o - 1.0) * _rtB->PS;

13396
      if (_rtB->Product1 < 1.0) {

13397
        /* '<S603>:1:12' */

13398
        /* '<S603>:1:13' */

13399
        _rtB->VC = sqrt((AFCS_MODEL1_rt_powd_snf(TrigonometricFunction4_o /

13400
          101325.0 + 1.0, 0.2857142857142857) - 1.0) * 5.0) * 340.294 * 3.6;

13401
      } else {

13402
        /* '<S603>:1:15' */

13403
        _rtB->VC = 0.0;

13404
      }

13405


13406
      _rtB->QC = TrigonometricFunction4_o;

13407


13408
      /* End of MATLAB Function: '<S464>/Embedded MATLAB Function' */

13409
    }

13410


13411
    if (ssIsSampleHit(S, 1, 0)) {

13412
    }

13413


13414
    if (1) {

13415
      /* Product: '<S617>/Product' */

13416
      _rtB->Product = 0.0 * _rtB->Saturation3;

13417
    }

13418


13419
    if (ssIsSampleHit(S, 1, 0)) {

13420
    }

13421


13422
    if (1) {

13423
      /* Product: '<S617>/Product7' */

13424
      _rtB->Product7 = 0.0 * _rtB->Gain1;

13425
    }

13426


13427
    if (ssIsSampleHit(S, 1, 0)) {

13428
    }

13429


13430
    if (1) {

13431
      /* Product: '<S617>/Product8' */

13432
      _rtB->Product8 = 0.0 * _rtB->Saturation4;

13433
    }

13434


13435
    if (ssIsSampleHit(S, 1, 0)) {

13436
    }

13437


13438
    if (1) {

13439
      /* Product: '<S466>/Product' */

13440
      _rtB->Product_g = _rtB->UnitConversion_oi * (real_T)

13441
        _rtB->NoseGearGeometryForcesandMoment.Switch;

13442
    }

13443


13444
    if (ssIsSampleHit(S, 1, 0)) {

13445
    }

13446


13447
    if (1) {

13448
      /* Product: '<S682>/Product' */

13449
      _rtB->jxk = _rtB->Backlash;

13450


13451
      /* Sum: '<S682>/Add' */

13452
      _rtB->jxk -= _rtB->UnitDelay1;

13453


13454
      /* Gain: '<S682>/Gain3' */

13455
      _rtB->jxk *= 15.0;

13456


13457
      /* Saturate: '<S682>/Saturation' */

13458
      if (_rtB->jxk > 50.0) {

13459
        _rtB->Saturation = 50.0;

13460
      } else if (_rtB->jxk < -50.0) {

13461
        _rtB->Saturation = -50.0;

13462
      } else {

13463
        _rtB->Saturation = _rtB->jxk;

13464
      }

13465


13466
      /* End of Saturate: '<S682>/Saturation' */

13467
    }

13468


13469
    if (ssIsSampleHit(S, 1, 0)) {

13470
    }

13471


13472
    if (1) {

13473
      /* Sum: '<S683>/Sum' */

13474
      _rtB->jxk = _rtB->Product_g - _rtB->UnitDelay1;

13475


13476
      /* Gain: '<S683>/Gain' */

13477
      _rtB->jxk *= 3.0;

13478


13479
      /* Saturate: '<S683>/Saturation1' */

13480
      if (_rtB->jxk > 12.0) {

13481
        _rtB->Saturation1 = 12.0;

13482
      } else if (_rtB->jxk < -12.0) {

13483
        _rtB->Saturation1 = -12.0;

13484
      } else {

13485
        _rtB->Saturation1 = _rtB->jxk;

13486
      }

13487


13488
      /* End of Saturate: '<S683>/Saturation1' */

13489


13490
      /* TransferFcn: '<S683>/Transfer Fcn2' */

13491
      _rtB->TransferFcn2 = 0.0;

13492
      _rtB->TransferFcn2 += 0.0 * _rtX->TransferFcn2_CSTATE[0];

13493
      _rtB->TransferFcn2 += 16.0 * _rtX->TransferFcn2_CSTATE[1];

13494


13495
      /* MATLAB Function: '<S469>/MATLAB Function' incorporates:

13496
       *  Constant: '<S14>/Constant1'

13497
       *  Constant: '<S14>/Constant2'

13498
       *  Constant: '<S469>/Constant'

13499
       */

13500
      /* MATLAB Function 'AFCS_MODEL1/Subsystem5/Subsystem3/Sensor/MATLAB Function': '<S769>:1' */

13501
      /* '<S769>:1:3' */

13502
      /* '<S769>:1:4' */

13503
      _rtB->Xnorth = (_rtB->Lat_p - 40.0) * 6.37E+6 * 0.017453292519943295;

13504


13505
      /* '<S769>:1:5' */

13506
      _rtB->Yeast = cos((_rtB->Lat_p + 40.0) * 0.017453292519943295 / 2.0) *

13507
        6.37E+6 * (_rtB->Lon_j - 30.0) * 0.017453292519943295;

13508


13509
      /* '<S769>:1:6' */

13510
      _rtB->mh = _rtB->Alt - _rtB->DataTypeConversion1;

13511


13512
      /* SignalConversion: '<S446>/OutportBufferForENT' */

13513
      _rtB->OutportBufferForENT[0] = _rtB->Xnorth;

13514
      _rtB->OutportBufferForENT[1] = _rtB->Yeast;

13515
      _rtB->OutportBufferForENT[2] = _rtB->mh;

13516


13517
      /* Integrator: '<S781>/Integrator' */

13518
      _rtB->jxk = _rtX->Integrator_CSTATE_a;

13519


13520
      /* Sum: '<S781>/Sum' */

13521
      _rtB->jxk += AFCS_MODEL1_rtC(S)->UnitConversion_b;

13522


13523
      /* Trigonometry: '<S775>/sincos' */

13524
      _rtB->kxi = cos(_rtB->jxk);

13525
      _rtB->jxk = sin(_rtB->jxk);

13526


13527
      /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn1' */

13528
      _rtB->VectorConcatenate_ak[0] = _rtB->kxi;

13529


13530
      /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn2' */

13531
      _rtB->VectorConcatenate_ak[1] = _rtB->jxk;

13532


13533
      /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn3' incorporates:

13534
       *  Constant: '<S775>/Zero'

13535
       */

13536
      _rtB->VectorConcatenate_ak[2] = 0.0;

13537


13538
      /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn5' */

13539
      _rtB->VectorConcatenate_ak[4] = _rtB->kxi;

13540


13541
      /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn6' incorporates:

13542
       *  Constant: '<S775>/Zero'

13543
       */

13544
      _rtB->VectorConcatenate_ak[5] = 0.0;

13545


13546
      /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn7' incorporates:

13547
       *  Constant: '<S775>/Zero'

13548
       */

13549
      _rtB->VectorConcatenate_ak[6] = 0.0;

13550


13551
      /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn8' incorporates:

13552
       *  Constant: '<S775>/Zero'

13553
       */

13554
      _rtB->VectorConcatenate_ak[7] = 0.0;

13555


13556
      /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn9' incorporates:

13557
       *  Constant: '<S775>/Zero1'

13558
       */

13559
      _rtB->VectorConcatenate_ak[8] = 1.0;

13560


13561
      /* UnaryMinus: '<S775>/Unary Minus' */

13562
      _rtB->VectorConcatenate_ak[3] = -_rtB->jxk;

13563


13564
      /* Math: '<S470>/Math Function1' */

13565
      for (i_1 = 0; i_1 < 3; i_1++) {

13566
        _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_n[i_1];

13567
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_n[i_1 + 3];

13568
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_n[i_1 + 6];

13569
      }

13570


13571
      /* End of Math: '<S470>/Math Function1' */

13572
      for (i_1 = 0; i_1 < 3; i_1++) {

13573
        /* Product: '<S470>/Product1' */

13574
        _rtB->Product1_o[i_1] = 0.0;

13575


13576
        /* Product: '<S795>/Product' */

13577
        _rtB->Sum_mx[i_1] = 0.0;

13578


13579
        /* Product: '<S796>/Product' incorporates:

13580
         *  Constant: '<S776>/Constant'

13581
         */

13582
        _rtB->Product1_f[i_1] = 0.0;

13583


13584
        /* Product: '<S470>/Product1' */

13585
        _rtB->Product1_o[i_1] += _rtB->MathFunction1_a[i_1] *

13586
          _rtB->UnitConversion_o[0];

13587


13588
        /* Product: '<S795>/Product' */

13589
        _rtB->Sum_mx[i_1] += AFCS_MODEL1_rtC(S)->VectorConcatenate[i_1] *

13590
          _rtB->Sum2_h[0];

13591


13592
        /* Product: '<S796>/Product' incorporates:

13593
         *  Constant: '<S776>/Constant'

13594
         */

13595
        _rtB->Product1_f[i_1] += 0.0 * _rtB->Sum2_h[0];

13596


13597
        /* Product: '<S470>/Product1' */

13598
        _rtB->Product1_o[i_1] += _rtB->MathFunction1_a[i_1 + 3] *

13599
          _rtB->UnitConversion_o[1];

13600


13601
        /* Product: '<S795>/Product' */

13602
        _rtB->Sum_mx[i_1] += AFCS_MODEL1_rtC(S)->VectorConcatenate[i_1 + 3] *

13603
          _rtB->Sum2_h[1];

13604


13605
        /* Product: '<S796>/Product' incorporates:

13606
         *  Constant: '<S776>/Constant'

13607
         */

13608
        _rtB->Product1_f[i_1] += 0.0 * _rtB->Sum2_h[1];

13609


13610
        /* Product: '<S470>/Product1' */

13611
        _rtB->Product1_o[i_1] += _rtB->MathFunction1_a[i_1 + 6] *

13612
          _rtB->UnitConversion_o[2];

13613


13614
        /* Product: '<S795>/Product' */

13615
        _rtB->Sum_mx[i_1] += AFCS_MODEL1_rtC(S)->VectorConcatenate[i_1 + 6] *

13616
          _rtB->Sum2_h[2];

13617


13618
        /* Product: '<S796>/Product' incorporates:

13619
         *  Constant: '<S776>/Constant'

13620
         */

13621
        _rtB->Product1_f[i_1] += 0.0 * _rtB->Sum2_h[2];

13622
      }

13623


13624
      /* Product: '<S798>/j x k' */

13625
      _rtB->jxk = _rtB->Sum2_h[1] * _rtB->Sum_mx[2];

13626


13627
      /* Product: '<S798>/k x i' */

13628
      _rtB->kxi = _rtB->Sum2_h[2] * _rtB->Sum_mx[0];

13629


13630
      /* Product: '<S798>/i x j' */

13631
      _rtB->ixj = _rtB->Sum2_h[0] * _rtB->Sum_mx[1];

13632


13633
      /* Product: '<S799>/k x j' */

13634
      _rtB->kxj = _rtB->Sum2_h[2] * _rtB->Sum_mx[1];

13635


13636
      /* Product: '<S799>/i x k' */

13637
      _rtB->ixk_a = _rtB->Sum2_h[0] * _rtB->Sum_mx[2];

13638


13639
      /* Product: '<S799>/j x i' */

13640
      _rtB->jxi_p = _rtB->Sum2_h[1] * _rtB->Sum_mx[0];

13641


13642
      /* Sum: '<S797>/Sum' */

13643
      _rtB->Sum_mx[0] = _rtB->jxk - _rtB->kxj;

13644
      _rtB->Sum_mx[1] = _rtB->kxi - _rtB->ixk_a;

13645
      _rtB->Sum_mx[2] = _rtB->ixj - _rtB->jxi_p;

13646


13647
      /* Sum: '<S777>/Sum2' */

13648
      _rtB->Product1_f[0] = (_rtB->Sum1_h[3] - _rtB->Product1_f[0]) -

13649
        _rtB->Sum_mx[0];

13650
      _rtB->Product1_f[1] = (_rtB->Sum1_h[4] - _rtB->Product1_f[1]) -

13651
        _rtB->Sum_mx[1];

13652
      _rtB->Product1_f[2] = (_rtB->Sum1_h[5] - _rtB->Product1_f[2]) -

13653
        _rtB->Sum_mx[2];

13654


13655
      /* Product: '<S777>/Product2' */

13656
      rt_mrdivide_U1d1x3_U2d_9vOrDY_k(_rtB->Product1_f, AFCS_MODEL1_rtC(S)

13657
        ->VectorConcatenate, _rtB->Sum_mx);

13658
      for (i_1 = 0; i_1 < 3; i_1++) {

13659
        /* Reshape: '<S777>/Reshape' */

13660
        _rtB->Reshape[i_1] = _rtB->Sum_mx[i_1];

13661


13662
        /* Math: '<S779>/Math Function' */

13663
        _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_ak[i_1];

13664
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_ak[i_1 + 3];

13665
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_ak[i_1 + 6];

13666
      }

13667


13668
      /* Product: '<S779>/Product1' */

13669
      for (i_1 = 0; i_1 < 3; i_1++) {

13670
        for (flag = 0; flag < 3; flag++) {

13671
          _rtB->MathFunction_k[i_1 + 3 * flag] = 0.0;

13672
          _rtB->MathFunction_k[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag]

13673
            * _rtB->VectorConcatenate_n[i_1];

13674
          _rtB->MathFunction_k[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag

13675
            + 1] * _rtB->VectorConcatenate_n[i_1 + 3];

13676
          _rtB->MathFunction_k[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag

13677
            + 2] * _rtB->VectorConcatenate_n[i_1 + 6];

13678
        }

13679
      }

13680


13681
      /* End of Product: '<S779>/Product1' */

13682


13683
      /* Math: '<S778>/Math Function' */

13684
      for (i_1 = 0; i_1 < 3; i_1++) {

13685
        j_2[3 * i_1] = _rtB->MathFunction_k[i_1];

13686
        j_2[1 + 3 * i_1] = _rtB->MathFunction_k[i_1 + 3];

13687
        j_2[2 + 3 * i_1] = _rtB->MathFunction_k[i_1 + 6];

13688
      }

13689


13690
      for (i_1 = 0; i_1 < 3; i_1++) {

13691
        /* Product: '<S778>/Product1' */

13692
        _rtB->Product1_e[i_1] = 0.0;

13693
        _rtB->MathFunction_k[3 * i_1] = j_2[3 * i_1];

13694


13695
        /* Math: '<S778>/Math Function1' */

13696
        _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_ak[i_1];

13697


13698
        /* Math: '<S778>/Math Function2' */

13699
        _rtB->MathFunction2[3 * i_1] = _rtB->VectorConcatenate_n[i_1];

13700


13701
        /* Product: '<S778>/Product1' */

13702
        _rtB->Product1_e[i_1] += _rtB->VectorConcatenate_ak[i_1] * _rtB->Sum_k[0];

13703
        _rtB->MathFunction_k[1 + 3 * i_1] = j_2[3 * i_1 + 1];

13704


13705
        /* Math: '<S778>/Math Function1' */

13706
        _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_ak[i_1 + 3];

13707


13708
        /* Math: '<S778>/Math Function2' */

13709
        _rtB->MathFunction2[1 + 3 * i_1] = _rtB->VectorConcatenate_n[i_1 + 3];

13710


13711
        /* Product: '<S778>/Product1' */

13712
        _rtB->Product1_e[i_1] += _rtB->VectorConcatenate_ak[i_1 + 3] *

13713
          _rtB->Sum_k[1];

13714
        _rtB->MathFunction_k[2 + 3 * i_1] = j_2[3 * i_1 + 2];

13715


13716
        /* Math: '<S778>/Math Function1' */

13717
        _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_ak[i_1 + 6];

13718


13719
        /* Math: '<S778>/Math Function2' */

13720
        _rtB->MathFunction2[2 + 3 * i_1] = _rtB->VectorConcatenate_n[i_1 + 6];

13721


13722
        /* Product: '<S778>/Product1' */

13723
        _rtB->Product1_e[i_1] += _rtB->VectorConcatenate_ak[i_1 + 6] *

13724
          _rtB->Sum_k[2];

13725
      }

13726


13727
      /* End of Math: '<S778>/Math Function' */

13728
      for (i_1 = 0; i_1 < 3; i_1++) {

13729
        /* Product: '<S778>/Product2' */

13730
        _rtB->Sum_mx[i_1] = 0.0;

13731


13732
        /* Product: '<S778>/Product4' */

13733
        _rtB->Product1_f[i_1] = 0.0;

13734


13735
        /* Product: '<S778>/Product5' */

13736
        _rtB->Product5[i_1] = 0.0;

13737


13738
        /* Product: '<S778>/Product2' */

13739
        _rtB->Sum_mx[i_1] += _rtB->MathFunction_k[i_1] * _rtB->ubvbwb[0];

13740


13741
        /* Product: '<S778>/Product4' */

13742
        _rtB->Product1_f[i_1] += _rtB->MathFunction1_a[i_1] * 0.0;

13743


13744
        /* Product: '<S778>/Product5' */

13745
        _rtB->Product5[i_1] += _rtB->MathFunction2[i_1] * _rtB->ubvbwb[0];

13746


13747
        /* Product: '<S778>/Product2' */

13748
        _rtB->Sum_mx[i_1] += _rtB->MathFunction_k[i_1 + 3] * _rtB->ubvbwb[1];

13749


13750
        /* Product: '<S778>/Product4' */

13751
        _rtB->Product1_f[i_1] += _rtB->MathFunction1_a[i_1 + 3] * 0.0;

13752


13753
        /* Product: '<S778>/Product5' */

13754
        _rtB->Product5[i_1] += _rtB->MathFunction2[i_1 + 3] * _rtB->ubvbwb[1];

13755


13756
        /* Product: '<S778>/Product2' */

13757
        _rtB->Sum_mx[i_1] += _rtB->MathFunction_k[i_1 + 6] * _rtB->ubvbwb[2];

13758


13759
        /* Product: '<S778>/Product4' */

13760
        _rtB->Product1_f[i_1] += _rtB->MathFunction1_a[i_1 + 6] * 7.292115E-5;

13761


13762
        /* Product: '<S778>/Product5' */

13763
        _rtB->Product5[i_1] += _rtB->MathFunction2[i_1 + 6] * _rtB->ubvbwb[2];

13764
      }

13765


13766
      /* Integrator: '<S778>/p' */

13767
      if (((int_T *)ssGetDWork(S, 350))[0] != 0) {

13768
        _rtX->p_CSTATE[0] = _rtB->Product1_e[0];

13769
        _rtX->p_CSTATE[1] = _rtB->Product1_e[1];

13770
        _rtX->p_CSTATE[2] = _rtB->Product1_e[2];

13771
      }

13772


13773
      _rtB->p_p[0] = _rtX->p_CSTATE[0];

13774
      _rtB->p_p[1] = _rtX->p_CSTATE[1];

13775
      _rtB->p_p[2] = _rtX->p_CSTATE[2];

13776


13777
      /* End of Integrator: '<S778>/p' */

13778


13779
      /* Product: '<S842>/j x k' */

13780
      _rtB->jxk = _rtB->p_p[1] * _rtB->Product1_f[2];

13781


13782
      /* Product: '<S842>/k x i' */

13783
      _rtB->kxi = _rtB->p_p[2] * _rtB->Product1_f[0];

13784


13785
      /* Product: '<S842>/i x j' */

13786
      _rtB->ixj = _rtB->p_p[0] * _rtB->Product1_f[1];

13787


13788
      /* Product: '<S843>/k x j' */

13789
      _rtB->kxj = _rtB->p_p[2] * _rtB->Product1_f[1];

13790


13791
      /* Product: '<S843>/i x k' */

13792
      _rtB->ixk_a = _rtB->p_p[0] * _rtB->Product1_f[2];

13793


13794
      /* Product: '<S843>/j x i' */

13795
      _rtB->jxi_p = _rtB->p_p[1] * _rtB->Product1_f[0];

13796


13797
      /* Sum: '<S801>/Sum' */

13798
      _rtB->Product1_f[0] = _rtB->jxk - _rtB->kxj;

13799
      _rtB->Product1_f[1] = _rtB->kxi - _rtB->ixk_a;

13800
      _rtB->Product1_f[2] = _rtB->ixj - _rtB->jxi_p;

13801


13802
      /* Sum: '<S778>/Sum2' */

13803
      _rtB->Sum2_e[0] = _rtB->Sum_mx[0] - _rtB->Product1_f[0];

13804
      _rtB->Sum2_e[1] = _rtB->Sum_mx[1] - _rtB->Product1_f[1];

13805
      _rtB->Sum2_e[2] = _rtB->Sum_mx[2] - _rtB->Product1_f[2];

13806


13807
      /* Sum: '<S904>/Add' */

13808
      _rtB->Add_n = (_rtB->Product4[0] + _rtB->Product4[4]) + _rtB->Product4[8];

13809


13810
      /* If: '<S848>/If' */

13811
      if (ssIsMajorTimeStep(S)) {

13812
        rtAction = (int8_T)!(_rtB->Add_n > 0.0);

13813
        ((int8_T *)ssGetDWork(S, 508))[0] = rtAction;

13814
      } else {

13815
        rtAction = ((int8_T *)ssGetDWork(S, 508))[0];

13816
      }

13817


13818
      switch (rtAction) {

13819
       case 0:

13820
        /* Outputs for IfAction SubSystem: '<S848>/Positive Trace' incorporates:

13821
         *  ActionPort: '<S903>/Action Port'

13822
         */

13823
        /* Sum: '<S903>/Sum' incorporates:

13824
         *  Constant: '<S903>/Constant'

13825
         */

13826
        _rtB->Gain1_g = _rtB->Add_n + 1.0;

13827


13828
        /* Sqrt: '<S903>/sqrt' */

13829
        _rtB->Gain1_g = sqrt(_rtB->Gain1_g);

13830


13831
        /* Gain: '<S903>/Gain1' */

13832
        _rtB->Gain1_g *= 2.0;

13833


13834
        /* Sum: '<S925>/Add' */

13835
        _rtB->Add_a = _rtB->Product4[7] - _rtB->Product4[5];

13836


13837
        /* Sum: '<S924>/Add' */

13838
        _rtB->Add_nms = _rtB->Product4[2] - _rtB->Product4[6];

13839


13840
        /* Sum: '<S926>/Add' */

13841
        _rtB->Add_fy = _rtB->Product4[3] - _rtB->Product4[1];

13842


13843
        /* End of Outputs for SubSystem: '<S848>/Positive Trace' */

13844
        break;

13845


13846
       case 1:

13847
        /* Outputs for IfAction SubSystem: '<S848>/Negative Trace' incorporates:

13848
         *  ActionPort: '<S902>/Action Port'

13849
         */

13850
        /* If: '<S902>/Find Maximum Diagonal Value' */

13851
        if (ssIsMajorTimeStep(S)) {

13852
          if ((_rtB->Product4[4] > _rtB->Product4[0]) && (_rtB->Product4[4] >

13853
               _rtB->Product4[8])) {

13854
            rtAction = 0;

13855
          } else if (_rtB->Product4[8] > _rtB->Product4[0]) {

13856
            rtAction = 1;

13857
          } else {

13858
            rtAction = 2;

13859
          }

13860


13861
          ((int8_T *)ssGetDWork(S, 509))[0] = rtAction;

13862
        } else {

13863
          rtAction = ((int8_T *)ssGetDWork(S, 509))[0];

13864
        }

13865


13866
        switch (rtAction) {

13867
         case 0:

13868
          /* Outputs for IfAction SubSystem: '<S902>/Maximum Value at DCM(2,2)' incorporates:

13869
           *  ActionPort: '<S906>/Action Port'

13870
           */

13871
          /* Sum: '<S918>/Add' incorporates:

13872
           *  Constant: '<S918>/Constant'

13873
           */

13874
          _rtB->Product_pv = ((_rtB->Product4[4] - _rtB->Product4[0]) -

13875
                              _rtB->Product4[8]) + 1.0;

13876


13877
          /* Sqrt: '<S906>/sqrt' */

13878
          _rtB->Product_pv = sqrt(_rtB->Product_pv);

13879


13880
          /* Sum: '<S916>/Add' */

13881
          _rtB->Add_j = _rtB->Product4[1] + _rtB->Product4[3];

13882


13883
          /* Sum: '<S915>/Add' */

13884
          _rtB->Add_e = _rtB->Product4[5] + _rtB->Product4[7];

13885


13886
          /* Sum: '<S914>/Add' */

13887
          _rtB->Add_g = _rtB->Product4[2] - _rtB->Product4[6];

13888


13889
          /* Switch: '<S917>/Switch' incorporates:

13890
           *  Constant: '<S917>/Constant1'

13891
           */

13892
          if (_rtB->Product_pv != 0.0) {

13893
            _rtB->Switch_j[0] = 0.5;

13894
            _rtB->Switch_j[1] = _rtB->Product_pv;

13895
          } else {

13896
            _rtB->Switch_j[0] = 0.0;

13897
            _rtB->Switch_j[1] = 1.0;

13898
          }

13899


13900
          /* End of Switch: '<S917>/Switch' */

13901


13902
          /* Product: '<S917>/Product' */

13903
          _rtB->Product_pv = _rtB->Switch_j[0] / _rtB->Switch_j[1];

13904


13905
          /* Product: '<S906>/Product' */

13906
          _rtB->Product_i[0] = _rtB->Add_j * _rtB->Product_pv;

13907
          _rtB->Product_i[1] = _rtB->Add_e * _rtB->Product_pv;

13908
          _rtB->Product_i[2] = _rtB->Add_g * _rtB->Product_pv;

13909


13910
          /* End of Outputs for SubSystem: '<S902>/Maximum Value at DCM(2,2)' */

13911
          break;

13912


13913
         case 1:

13914
          /* Outputs for IfAction SubSystem: '<S902>/Maximum Value at DCM(3,3)' incorporates:

13915
           *  ActionPort: '<S907>/Action Port'

13916
           */

13917
          /* Sum: '<S923>/Add' incorporates:

13918
           *  Constant: '<S923>/Constant'

13919
           */

13920
          _rtB->Product_l = ((_rtB->Product4[8] - _rtB->Product4[0]) -

13921
                             _rtB->Product4[4]) + 1.0;

13922


13923
          /* Sqrt: '<S907>/sqrt' */

13924
          _rtB->Product_l = sqrt(_rtB->Product_l);

13925


13926
          /* Sum: '<S919>/Add' */

13927
          _rtB->Add_nm = _rtB->Product4[2] + _rtB->Product4[6];

13928


13929
          /* Sum: '<S920>/Add' */

13930
          _rtB->Add_f = _rtB->Product4[5] + _rtB->Product4[7];

13931


13932
          /* Sum: '<S921>/Add' */

13933
          _rtB->Add_c = _rtB->Product4[3] - _rtB->Product4[1];

13934


13935
          /* Switch: '<S922>/Switch' incorporates:

13936
           *  Constant: '<S922>/Constant1'

13937
           */

13938
          if (_rtB->Product_l != 0.0) {

13939
            _rtB->Switch_d[0] = 0.5;

13940
            _rtB->Switch_d[1] = _rtB->Product_l;

13941
          } else {

13942
            _rtB->Switch_d[0] = 0.0;

13943
            _rtB->Switch_d[1] = 1.0;

13944
          }

13945


13946
          /* End of Switch: '<S922>/Switch' */

13947


13948
          /* Product: '<S922>/Product' */

13949
          _rtB->Product_l = _rtB->Switch_d[0] / _rtB->Switch_d[1];

13950


13951
          /* Product: '<S907>/Product' */

13952
          _rtB->Product_o[0] = _rtB->Add_nm * _rtB->Product_l;

13953
          _rtB->Product_o[1] = _rtB->Add_f * _rtB->Product_l;

13954
          _rtB->Product_o[2] = _rtB->Add_c * _rtB->Product_l;

13955


13956
          /* End of Outputs for SubSystem: '<S902>/Maximum Value at DCM(3,3)' */

13957
          break;

13958


13959
         case 2:

13960
          /* Outputs for IfAction SubSystem: '<S902>/Maximum Value at DCM(1,1)' incorporates:

13961
           *  ActionPort: '<S905>/Action Port'

13962
           */

13963
          /* Sum: '<S913>/Add' incorporates:

13964
           *  Constant: '<S913>/Constant'

13965
           */

13966
          _rtB->Product_m = ((_rtB->Product4[0] - _rtB->Product4[4]) -

13967
                             _rtB->Product4[8]) + 1.0;

13968


13969
          /* Sqrt: '<S905>/sqrt' */

13970
          _rtB->Product_m = sqrt(_rtB->Product_m);

13971


13972
          /* Switch: '<S912>/Switch' incorporates:

13973
           *  Constant: '<S912>/Constant1'

13974
           */

13975
          if (_rtB->Product_m != 0.0) {

13976
            _rtB->Switch_f[0] = 0.5;

13977
            _rtB->Switch_f[1] = _rtB->Product_m;

13978
          } else {

13979
            _rtB->Switch_f[0] = 0.0;

13980
            _rtB->Switch_f[1] = 1.0;

13981
          }

13982


13983
          /* End of Switch: '<S912>/Switch' */

13984


13985
          /* Product: '<S912>/Product' */

13986
          _rtB->Product_m = _rtB->Switch_f[0] / _rtB->Switch_f[1];

13987


13988
          /* Sum: '<S911>/Add' */

13989
          _rtB->Add_k = _rtB->Product4[1] + _rtB->Product4[3];

13990


13991
          /* Sum: '<S909>/Add' */

13992
          _rtB->Add_kg = _rtB->Product4[2] + _rtB->Product4[6];

13993


13994
          /* Sum: '<S910>/Add' */

13995
          _rtB->Add_p = _rtB->Product4[7] - _rtB->Product4[5];

13996


13997
          /* Product: '<S905>/Product' */

13998
          _rtB->Product_gm[0] = _rtB->Product_m * _rtB->Add_k;

13999
          _rtB->Product_gm[1] = _rtB->Product_m * _rtB->Add_kg;

14000
          _rtB->Product_gm[2] = _rtB->Product_m * _rtB->Add_p;

14001


14002
          /* End of Outputs for SubSystem: '<S902>/Maximum Value at DCM(1,1)' */

14003
          break;

14004
        }

14005


14006
        /* End of If: '<S902>/Find Maximum Diagonal Value' */

14007
        /* End of Outputs for SubSystem: '<S848>/Negative Trace' */

14008
        break;

14009
      }

14010


14011
      /* End of If: '<S848>/If' */

14012


14013
      /* DotProduct: '<S851>/Dot Product' */

14014
      _rtB->jxk = ((_rtB->q_j[0] * _rtB->q_j[0] + _rtB->q_j[1] * _rtB->q_j[1]) +

14015
                   _rtB->q_j[2] * _rtB->q_j[2]) + _rtB->q_j[3] * _rtB->q_j[3];

14016


14017
      /* Sum: '<S851>/Sum' incorporates:

14018
       *  Constant: '<S851>/Constant'

14019
       */

14020
      _rtB->jxk = 1.0 - _rtB->jxk;

14021


14022
      /* Sum: '<S470>/Sum4' */

14023
      _rtB->Sum4[0] = _rtB->Sum2_h[0] - _rtB->Sum4[0];

14024
      _rtB->Sum4[1] = _rtB->Sum2_h[1] - _rtB->Sum4[1];

14025
      _rtB->Sum4[2] = _rtB->Sum2_h[2] - _rtB->Sum4[2];

14026


14027
      /* Fcn: '<S851>/q0dot' */

14028
      _rtB->q0dot = ((_rtB->q_j[1] * _rtB->Sum4[0] + _rtB->q_j[2] * _rtB->Sum4[1])

14029
                     + _rtB->q_j[3] * _rtB->Sum4[2]) * -0.5 + _rtB->jxk *

14030
        _rtB->q_j[0];

14031


14032
      /* Fcn: '<S851>/q1dot' */

14033
      _rtB->q1dot = ((_rtB->q_j[0] * _rtB->Sum4[0] + _rtB->q_j[2] * _rtB->Sum4[2])

14034
                     - _rtB->q_j[3] * _rtB->Sum4[1]) * 0.5 + _rtB->jxk *

14035
        _rtB->q_j[1];

14036


14037
      /* Fcn: '<S851>/q2dot' */

14038
      _rtB->q2dot = ((_rtB->q_j[0] * _rtB->Sum4[1] + _rtB->q_j[3] * _rtB->Sum4[0])

14039
                     - _rtB->q_j[1] * _rtB->Sum4[2]) * 0.5 + _rtB->jxk *

14040
        _rtB->q_j[2];

14041


14042
      /* Fcn: '<S851>/q3dot' */

14043
      _rtB->q3dot = ((_rtB->q_j[0] * _rtB->Sum4[2] + _rtB->q_j[1] * _rtB->Sum4[1])

14044
                     - _rtB->q_j[2] * _rtB->Sum4[0]) * 0.5 + _rtB->jxk *

14045
        _rtB->q_j[3];

14046


14047
      /* Product: '<S948>/j x k' */

14048
      _rtB->jxk = 0.0 * _rtB->p1[2];

14049


14050
      /* Product: '<S948>/k x i' */

14051
      _rtB->kxi = 7.292115E-5 * _rtB->p1[0];

14052


14053
      /* Product: '<S948>/i x j' */

14054
      _rtB->ixj = 0.0 * _rtB->p1[1];

14055


14056
      /* Product: '<S949>/k x j' */

14057
      _rtB->kxj = 7.292115E-5 * _rtB->p1[1];

14058


14059
      /* Product: '<S949>/i x k' */

14060
      _rtB->ixk_a = 0.0 * _rtB->p1[2];

14061


14062
      /* Product: '<S949>/j x i' */

14063
      _rtB->jxi_p = 0.0 * _rtB->p1[0];

14064


14065
      /* Sum: '<S943>/Sum' */

14066
      _rtB->Sum_mx[0] = _rtB->jxk - _rtB->kxj;

14067
      _rtB->Sum_mx[1] = _rtB->kxi - _rtB->ixk_a;

14068
      _rtB->Sum_mx[2] = _rtB->ixj - _rtB->jxi_p;

14069


14070
      /* Product: '<S946>/j x k' */

14071
      _rtB->jxk = 0.0 * _rtB->Sum_mx[2];

14072


14073
      /* Product: '<S946>/k x i' */

14074
      _rtB->kxi = 7.292115E-5 * _rtB->Sum_mx[0];

14075


14076
      /* Product: '<S946>/i x j' */

14077
      _rtB->ixj = 0.0 * _rtB->Sum_mx[1];

14078


14079
      /* Product: '<S947>/k x j' */

14080
      _rtB->kxj = 7.292115E-5 * _rtB->Sum_mx[1];

14081


14082
      /* Product: '<S947>/i x k' */

14083
      _rtB->ixk_a = 0.0 * _rtB->Sum_mx[2];

14084


14085
      /* Product: '<S947>/j x i' */

14086
      _rtB->jxi_p = 0.0 * _rtB->Sum_mx[0];

14087


14088
      /* Sum: '<S942>/Sum' */

14089
      _rtB->Sum_mx[0] = _rtB->jxk - _rtB->kxj;

14090
      _rtB->Sum_mx[1] = _rtB->kxi - _rtB->ixk_a;

14091
      _rtB->Sum_mx[2] = _rtB->ixj - _rtB->jxi_p;

14092


14093
      /* Product: '<S780>/Product1' */

14094
      for (i_1 = 0; i_1 < 3; i_1++) {

14095
        _rtB->Product1_f[i_1] = 0.0;

14096
        _rtB->Product1_f[i_1] += _rtB->VectorConcatenate_n[i_1] * _rtB->Sum_mx[0];

14097
        _rtB->Product1_f[i_1] += _rtB->VectorConcatenate_n[i_1 + 3] *

14098
          _rtB->Sum_mx[1];

14099
        _rtB->Product1_f[i_1] += _rtB->VectorConcatenate_n[i_1 + 6] *

14100
          _rtB->Sum_mx[2];

14101
      }

14102


14103
      /* End of Product: '<S780>/Product1' */

14104
      for (i_1 = 0; i_1 < 3; i_1++) {

14105
        /* Product: '<S780>/Product2' */

14106
        _rtB->Sum_mx[i_1] = 0.0;

14107
        _rtB->Sum_mx[i_1] += _rtB->VectorConcatenate_n[i_1] * 0.0;

14108
        _rtB->Sum_mx[i_1] += _rtB->VectorConcatenate_n[i_1 + 3] * 0.0;

14109
        _rtB->Sum_mx[i_1] += _rtB->VectorConcatenate_n[i_1 + 6] * 7.292115E-5;

14110


14111
        /* Sum: '<S780>/Sum2' */

14112
        _rtB->Sum2_h[i_1] += _rtB->Sum_mx[i_1];

14113
      }

14114


14115
      /* Product: '<S944>/j x k' */

14116
      _rtB->jxk = _rtB->ubvbwb[1] * _rtB->Sum2_h[2];

14117


14118
      /* Product: '<S944>/k x i' */

14119
      _rtB->kxi = _rtB->ubvbwb[2] * _rtB->Sum2_h[0];

14120


14121
      /* Product: '<S944>/i x j' */

14122
      _rtB->ixj = _rtB->ubvbwb[0] * _rtB->Sum2_h[1];

14123


14124
      /* Product: '<S945>/k x j' */

14125
      _rtB->kxj = _rtB->ubvbwb[2] * _rtB->Sum2_h[1];

14126


14127
      /* Product: '<S945>/i x k' */

14128
      _rtB->ixk_a = _rtB->ubvbwb[0] * _rtB->Sum2_h[2];

14129


14130
      /* Product: '<S945>/j x i' */

14131
      _rtB->jxi_p = _rtB->ubvbwb[1] * _rtB->Sum2_h[0];

14132


14133
      /* Sum: '<S941>/Sum' */

14134
      _rtB->Sum_mx[0] = _rtB->jxk - _rtB->kxj;

14135
      _rtB->Sum_mx[1] = _rtB->kxi - _rtB->ixk_a;

14136
      _rtB->Sum_mx[2] = _rtB->ixj - _rtB->jxi_p;

14137


14138
      /* Sum: '<S780>/Sum' */

14139
      _rtB->Sum_e[0] = (_rtB->Sum_mx[0] - _rtB->Product1_f[0]) + _rtB->

14140
        Product_d[0];

14141
      _rtB->Sum_e[1] = (_rtB->Sum_mx[1] - _rtB->Product1_f[1]) + _rtB->

14142
        Product_d[1];

14143
      _rtB->Sum_e[2] = (_rtB->Sum_mx[2] - _rtB->Product1_f[2]) + _rtB->

14144
        Product_d[2];

14145


14146
      /* TransferFcn: '<S446>/Transfer Fcn' */

14147
      _rtB->TransferFcn = 0.0;

14148
      _rtB->TransferFcn += -2500.0 * _rtX->TransferFcn_CSTATE;

14149
      _rtB->TransferFcn += 50.0 * _rtB->Switch_h;

14150
    }

14151
  }

14152


14153
  /* End of Outputs for SubSystem: '<S15>/Subsystem3' */

14154
  if (ssIsSampleHit(S, 1, 0)) {

14155
  }

14156


14157
  if (1) {

14158
    /* Gain: '<S448>/Gain14' */

14159
    _rtB->Gain14 = 1000.0 * _rtB->Altitude_j;

14160
  }

14161


14162
  if (ssIsSampleHit(S, 1, 0)) {

14163
    /* Gain: '<S449>/Gain14' */

14164
    _rtB->Gain14_b = 1000.0 * _rtB->Gain11;

14165


14166
    /* Gain: '<S449>/Gain12' incorporates:

14167
     *  Constant: '<S14>/Constant2'

14168
     */

14169
    _rtB->Gain13_d = 4.0E+8;

14170


14171
    /* DataTypeConversion: '<S449>/Data Type Conversion13' */

14172
    TrigonometricFunction4_o = floor(_rtB->Gain13_d);

14173
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14174
    {

14175
      TrigonometricFunction4_o = 0.0;

14176
    } else {

14177
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14178
    }

14179


14180
    _rtB->Lat_c = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14181
      -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14182


14183
    /* End of DataTypeConversion: '<S449>/Data Type Conversion13' */

14184


14185
    /* Gain: '<S449>/Gain13' incorporates:

14186
     *  Constant: '<S14>/Constant1'

14187
     */

14188
    _rtB->Gain13_d = 3.0E+8;

14189


14190
    /* DataTypeConversion: '<S449>/Data Type Conversion14' */

14191
    TrigonometricFunction4_o = floor(_rtB->Gain13_d);

14192
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14193
    {

14194
      TrigonometricFunction4_o = 0.0;

14195
    } else {

14196
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14197
    }

14198


14199
    _rtB->Lon_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14200
      -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14201


14202
    /* End of DataTypeConversion: '<S449>/Data Type Conversion14' */

14203


14204
    /* DataTypeConversion: '<S449>/Data Type Conversion15' */

14205
    TrigonometricFunction4_o = floor(_rtB->Gain14_b);

14206
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14207
    {

14208
      TrigonometricFunction4_o = 0.0;

14209
    } else {

14210
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14211
    }

14212


14213
    _rtB->Alt_e = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14214
      -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14215


14216
    /* End of DataTypeConversion: '<S449>/Data Type Conversion15' */

14217
  }

14218


14219
  if (1) {

14220
    /* Switch: '<S445>/Switch' incorporates:

14221
     *  Switch: '<S445>/Switch'

14222
     */

14223
    if (_rtB->takeoff > 0) {

14224
      /* Gain: '<S448>/Gain' */

14225
      _rtB->Gain_d = 1000.0 * _rtB->phiL;

14226


14227
      /* DataTypeConversion: '<S448>/Data Type Conversion10' */

14228
      TrigonometricFunction4_o = floor(_rtB->Gain_d);

14229
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14230
      {

14231
        TrigonometricFunction4_o = 0.0;

14232
      } else {

14233
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14234
      }

14235


14236
      _rtB->phiL_k = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14237
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14238


14239
      /* End of DataTypeConversion: '<S448>/Data Type Conversion10' */

14240
      _rtB->phiL_j = _rtB->phiL_k;

14241


14242
      /* Gain: '<S448>/Gain9' */

14243
      _rtB->Gain9 = 1000.0 * _rtB->GPS_vnorth_i;

14244


14245
      /* DataTypeConversion: '<S448>/Data Type Conversion9' */

14246
      TrigonometricFunction4_o = floor(_rtB->Gain9);

14247
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14248
      {

14249
        TrigonometricFunction4_o = 0.0;

14250
      } else {

14251
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14252
      }

14253


14254
      _rtB->Vn_c = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14255
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14256


14257
      /* End of DataTypeConversion: '<S448>/Data Type Conversion9' */

14258
      _rtB->Vn = _rtB->Vn_c;

14259


14260
      /* Gain: '<S448>/Gain10' */

14261
      _rtB->Gain10 = 1000.0 * _rtB->GPS_veast_k;

14262


14263
      /* DataTypeConversion: '<S448>/Data Type Conversion11' */

14264
      TrigonometricFunction4_o = floor(_rtB->Gain10);

14265
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14266
      {

14267
        TrigonometricFunction4_o = 0.0;

14268
      } else {

14269
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14270
      }

14271


14272
      _rtB->Ve_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14273
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14274


14275
      /* End of DataTypeConversion: '<S448>/Data Type Conversion11' */

14276
      _rtB->Ve = _rtB->Ve_a;

14277


14278
      /* Gain: '<S448>/Gain11' */

14279
      _rtB->Gain11_h = 1000.0 * _rtB->GPS_vdown_k;

14280


14281
      /* DataTypeConversion: '<S448>/Data Type Conversion12' */

14282
      TrigonometricFunction4_o = floor(_rtB->Gain11_h);

14283
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14284
      {

14285
        TrigonometricFunction4_o = 0.0;

14286
      } else {

14287
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14288
      }

14289


14290
      _rtB->Vd_j = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14291
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14292


14293
      /* End of DataTypeConversion: '<S448>/Data Type Conversion12' */

14294
      _rtB->Vd = _rtB->Vd_j;

14295


14296
      /* Gain: '<S448>/Gain12' */

14297
      _rtB->Gain12 = 1.0E+7 * _rtB->Lat_j;

14298


14299
      /* DataTypeConversion: '<S448>/Data Type Conversion13' */

14300
      TrigonometricFunction4_o = floor(_rtB->Gain12);

14301
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14302
      {

14303
        TrigonometricFunction4_o = 0.0;

14304
      } else {

14305
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14306
      }

14307


14308
      _rtB->Lat_i = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14309
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14310


14311
      /* End of DataTypeConversion: '<S448>/Data Type Conversion13' */

14312
      _rtB->Lat_pm = _rtB->Lat_i;

14313


14314
      /* Gain: '<S448>/Gain13' */

14315
      _rtB->Gain13 = 1.0E+7 * _rtB->Lon_e;

14316


14317
      /* DataTypeConversion: '<S448>/Data Type Conversion14' */

14318
      TrigonometricFunction4_o = floor(_rtB->Gain13);

14319
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14320
      {

14321
        TrigonometricFunction4_o = 0.0;

14322
      } else {

14323
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14324
      }

14325


14326
      _rtB->Lon_eh = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14327
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14328


14329
      /* End of DataTypeConversion: '<S448>/Data Type Conversion14' */

14330
      _rtB->Lon_m = _rtB->Lon_eh;

14331


14332
      /* DataTypeConversion: '<S448>/Data Type Conversion15' */

14333
      TrigonometricFunction4_o = floor(_rtB->Gain14);

14334
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14335
      {

14336
        TrigonometricFunction4_o = 0.0;

14337
      } else {

14338
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14339
      }

14340


14341
      _rtB->Alt_oj = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14342
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14343


14344
      /* End of DataTypeConversion: '<S448>/Data Type Conversion15' */

14345
      _rtB->Alt_o3 = _rtB->Alt_oj;

14346


14347
      /* Gain: '<S448>/Gain15' */

14348
      _rtB->Gain15 = 1000.0 * _rtB->Alfa;

14349


14350
      /* DataTypeConversion: '<S448>/Data Type Conversion16' */

14351
      TrigonometricFunction4_o = floor(_rtB->Gain15);

14352
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14353
      {

14354
        TrigonometricFunction4_o = 0.0;

14355
      } else {

14356
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14357
      }

14358


14359
      _rtB->Ndefine1_k = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14360
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14361


14362
      /* End of DataTypeConversion: '<S448>/Data Type Conversion16' */

14363
      _rtB->Ndefine1 = _rtB->Ndefine1_k;

14364


14365
      /* Gain: '<S448>/Gain16' */

14366
      _rtB->Gain16 = 1000.0 * _rtB->Beta;

14367


14368
      /* DataTypeConversion: '<S448>/Data Type Conversion17' */

14369
      TrigonometricFunction4_o = floor(_rtB->Gain16);

14370
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14371
      {

14372
        TrigonometricFunction4_o = 0.0;

14373
      } else {

14374
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14375
      }

14376


14377
      _rtB->Ndefine2_o = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14378
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14379


14380
      /* End of DataTypeConversion: '<S448>/Data Type Conversion17' */

14381
      _rtB->Ndefine2 = _rtB->Ndefine2_o;

14382


14383
      /* Gain: '<S448>/Gain17' */

14384
      _rtB->Gain17 = 1000.0 * _rtB->Qbar;

14385


14386
      /* DataTypeConversion: '<S448>/Data Type Conversion18' */

14387
      TrigonometricFunction4_o = floor(_rtB->Gain17);

14388
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14389
      {

14390
        TrigonometricFunction4_o = 0.0;

14391
      } else {

14392
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14393
      }

14394


14395
      _rtB->Ndefine3_c = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14396
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14397


14398
      /* End of DataTypeConversion: '<S448>/Data Type Conversion18' */

14399
      _rtB->Ndefine3 = _rtB->Ndefine3_c;

14400


14401
      /* Gain: '<S448>/Gain18' */

14402
      _rtB->Gain18 = 1000.0 * _rtB->Va;

14403


14404
      /* DataTypeConversion: '<S448>/Data Type Conversion19' */

14405
      TrigonometricFunction4_o = floor(_rtB->Gain18);

14406
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14407
      {

14408
        TrigonometricFunction4_o = 0.0;

14409
      } else {

14410
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14411
      }

14412


14413
      _rtB->Va_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14414
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14415


14416
      /* End of DataTypeConversion: '<S448>/Data Type Conversion19' */

14417
      _rtB->Va_k = _rtB->Va_m;

14418


14419
      /* Gain: '<S448>/Gain1' */

14420
      _rtB->Gain1_p = 1000.0 * _rtB->thetaL;

14421


14422
      /* DataTypeConversion: '<S448>/Data Type Conversion1' */

14423
      TrigonometricFunction4_o = floor(_rtB->Gain1_p);

14424
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14425
      {

14426
        TrigonometricFunction4_o = 0.0;

14427
      } else {

14428
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14429
      }

14430


14431
      _rtB->thetaL_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14432
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14433


14434
      /* End of DataTypeConversion: '<S448>/Data Type Conversion1' */

14435
      _rtB->thetaL_d = _rtB->thetaL_m;

14436


14437
      /* Gain: '<S448>/Gain2' */

14438
      _rtB->Gain2_i = 1000.0 * _rtB->psiL;

14439


14440
      /* DataTypeConversion: '<S448>/Data Type Conversion2' */

14441
      TrigonometricFunction4_o = floor(_rtB->Gain2_i);

14442
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14443
      {

14444
        TrigonometricFunction4_o = 0.0;

14445
      } else {

14446
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14447
      }

14448


14449
      _rtB->psiL_f = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14450
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14451


14452
      /* End of DataTypeConversion: '<S448>/Data Type Conversion2' */

14453
      _rtB->psiL_j = _rtB->psiL_f;

14454


14455
      /* Gain: '<S448>/Gain3' */

14456
      _rtB->Gain3_b = 1000.0 * _rtB->p;

14457


14458
      /* DataTypeConversion: '<S448>/Data Type Conversion3' */

14459
      TrigonometricFunction4_o = floor(_rtB->Gain3_b);

14460
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14461
      {

14462
        TrigonometricFunction4_o = 0.0;

14463
      } else {

14464
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14465
      }

14466


14467
      _rtB->p_l = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14468
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14469


14470
      /* End of DataTypeConversion: '<S448>/Data Type Conversion3' */

14471
      _rtB->p_pm = _rtB->p_l;

14472


14473
      /* Gain: '<S448>/Gain4' */

14474
      _rtB->Gain4 = 1000.0 * _rtB->q;

14475


14476
      /* DataTypeConversion: '<S448>/Data Type Conversion4' */

14477
      TrigonometricFunction4_o = floor(_rtB->Gain4);

14478
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14479
      {

14480
        TrigonometricFunction4_o = 0.0;

14481
      } else {

14482
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14483
      }

14484


14485
      _rtB->q_g = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14486
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14487


14488
      /* End of DataTypeConversion: '<S448>/Data Type Conversion4' */

14489
      _rtB->q_je = _rtB->q_g;

14490


14491
      /* Gain: '<S448>/Gain5' */

14492
      _rtB->Gain5 = 1000.0 * _rtB->r;

14493


14494
      /* DataTypeConversion: '<S448>/Data Type Conversion5' */

14495
      TrigonometricFunction4_o = floor(_rtB->Gain5);

14496
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14497
      {

14498
        TrigonometricFunction4_o = 0.0;

14499
      } else {

14500
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14501
      }

14502


14503
      _rtB->r_o = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14504
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14505


14506
      /* End of DataTypeConversion: '<S448>/Data Type Conversion5' */

14507
      _rtB->r_l = _rtB->r_o;

14508


14509
      /* Gain: '<S448>/Gain6' */

14510
      _rtB->Gain6 = 1000.0 * _rtB->Abx;

14511


14512
      /* DataTypeConversion: '<S448>/Data Type Conversion6' */

14513
      TrigonometricFunction4_o = floor(_rtB->Gain6);

14514
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14515
      {

14516
        TrigonometricFunction4_o = 0.0;

14517
      } else {

14518
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14519
      }

14520


14521
      _rtB->Abx_n = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14522
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14523


14524
      /* End of DataTypeConversion: '<S448>/Data Type Conversion6' */

14525
      _rtB->Abx_o = _rtB->Abx_n;

14526


14527
      /* Gain: '<S448>/Gain7' */

14528
      _rtB->Gain7 = 1000.0 * _rtB->Aby;

14529


14530
      /* DataTypeConversion: '<S448>/Data Type Conversion7' */

14531
      TrigonometricFunction4_o = floor(_rtB->Gain7);

14532
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14533
      {

14534
        TrigonometricFunction4_o = 0.0;

14535
      } else {

14536
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14537
      }

14538


14539
      _rtB->Aby_l = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14540
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14541


14542
      /* End of DataTypeConversion: '<S448>/Data Type Conversion7' */

14543
      _rtB->Aby_a = _rtB->Aby_l;

14544


14545
      /* Gain: '<S448>/Gain8' */

14546
      _rtB->Gain8 = 1000.0 * _rtB->Abz;

14547


14548
      /* DataTypeConversion: '<S448>/Data Type Conversion8' */

14549
      TrigonometricFunction4_o = floor(_rtB->Gain8);

14550
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14551
      {

14552
        TrigonometricFunction4_o = 0.0;

14553
      } else {

14554
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14555
      }

14556


14557
      _rtB->Abz_d = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14558
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14559


14560
      /* End of DataTypeConversion: '<S448>/Data Type Conversion8' */

14561
      _rtB->Abz_h = _rtB->Abz_d;

14562
    } else {

14563
      /* Gain: '<S449>/Gain' */

14564
      _rtB->Gain_a = 1000.0 * _rtB->phiL;

14565


14566
      /* DataTypeConversion: '<S449>/Data Type Conversion10' */

14567
      TrigonometricFunction4_o = floor(_rtB->Gain_a);

14568
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14569
      {

14570
        TrigonometricFunction4_o = 0.0;

14571
      } else {

14572
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14573
      }

14574


14575
      _rtB->phiL_d = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14576
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14577


14578
      /* End of DataTypeConversion: '<S449>/Data Type Conversion10' */

14579
      _rtB->phiL_j = _rtB->phiL_d;

14580


14581
      /* Gain: '<S449>/Gain9' */

14582
      _rtB->Gain9_j = 1000.0 * _rtB->GPS_vnorth_i;

14583


14584
      /* DataTypeConversion: '<S449>/Data Type Conversion9' */

14585
      TrigonometricFunction4_o = floor(_rtB->Gain9_j);

14586
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14587
      {

14588
        TrigonometricFunction4_o = 0.0;

14589
      } else {

14590
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14591
      }

14592


14593
      _rtB->Vn_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14594
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14595


14596
      /* End of DataTypeConversion: '<S449>/Data Type Conversion9' */

14597
      _rtB->Vn = _rtB->Vn_m;

14598


14599
      /* Gain: '<S449>/Gain10' */

14600
      _rtB->Gain10_k = 1000.0 * _rtB->GPS_veast_k;

14601


14602
      /* DataTypeConversion: '<S449>/Data Type Conversion11' */

14603
      TrigonometricFunction4_o = floor(_rtB->Gain10_k);

14604
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14605
      {

14606
        TrigonometricFunction4_o = 0.0;

14607
      } else {

14608
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14609
      }

14610


14611
      _rtB->Ve_p = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14612
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14613


14614
      /* End of DataTypeConversion: '<S449>/Data Type Conversion11' */

14615
      _rtB->Ve = _rtB->Ve_p;

14616


14617
      /* Gain: '<S449>/Gain11' */

14618
      _rtB->Gain11_a = 1000.0 * _rtB->GPS_vdown_k;

14619


14620
      /* DataTypeConversion: '<S449>/Data Type Conversion12' */

14621
      TrigonometricFunction4_o = floor(_rtB->Gain11_a);

14622
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14623
      {

14624
        TrigonometricFunction4_o = 0.0;

14625
      } else {

14626
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14627
      }

14628


14629
      _rtB->Vd_o = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14630
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14631


14632
      /* End of DataTypeConversion: '<S449>/Data Type Conversion12' */

14633
      _rtB->Vd = _rtB->Vd_o;

14634
      _rtB->Lat_pm = _rtB->Lat_c;

14635
      _rtB->Lon_m = _rtB->Lon_a;

14636
      _rtB->Alt_o3 = _rtB->Alt_e;

14637


14638
      /* Gain: '<S449>/Gain15' */

14639
      _rtB->Gain15_n = 1000.0 * _rtB->Alfa;

14640


14641
      /* DataTypeConversion: '<S449>/Data Type Conversion16' */

14642
      TrigonometricFunction4_o = floor(_rtB->Gain15_n);

14643
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14644
      {

14645
        TrigonometricFunction4_o = 0.0;

14646
      } else {

14647
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14648
      }

14649


14650
      _rtB->Ndefine1_o = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14651
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14652


14653
      /* End of DataTypeConversion: '<S449>/Data Type Conversion16' */

14654
      _rtB->Ndefine1 = _rtB->Ndefine1_o;

14655


14656
      /* Gain: '<S449>/Gain16' */

14657
      _rtB->Gain16_d = 1000.0 * _rtB->Beta;

14658


14659
      /* DataTypeConversion: '<S449>/Data Type Conversion17' */

14660
      TrigonometricFunction4_o = floor(_rtB->Gain16_d);

14661
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14662
      {

14663
        TrigonometricFunction4_o = 0.0;

14664
      } else {

14665
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14666
      }

14667


14668
      _rtB->Ndefine2_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14669
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14670


14671
      /* End of DataTypeConversion: '<S449>/Data Type Conversion17' */

14672
      _rtB->Ndefine2 = _rtB->Ndefine2_m;

14673


14674
      /* Gain: '<S449>/Gain17' */

14675
      _rtB->Gain17_i = 1000.0 * _rtB->Qbar;

14676


14677
      /* DataTypeConversion: '<S449>/Data Type Conversion18' */

14678
      TrigonometricFunction4_o = floor(_rtB->Gain17_i);

14679
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14680
      {

14681
        TrigonometricFunction4_o = 0.0;

14682
      } else {

14683
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14684
      }

14685


14686
      _rtB->Ndefine3_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14687
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14688


14689
      /* End of DataTypeConversion: '<S449>/Data Type Conversion18' */

14690
      _rtB->Ndefine3 = _rtB->Ndefine3_a;

14691


14692
      /* Gain: '<S449>/Gain18' */

14693
      _rtB->Gain18_b = 1000.0 * _rtB->Va;

14694


14695
      /* DataTypeConversion: '<S449>/Data Type Conversion19' */

14696
      TrigonometricFunction4_o = floor(_rtB->Gain18_b);

14697
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14698
      {

14699
        TrigonometricFunction4_o = 0.0;

14700
      } else {

14701
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14702
      }

14703


14704
      _rtB->Va_f = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14705
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14706


14707
      /* End of DataTypeConversion: '<S449>/Data Type Conversion19' */

14708
      _rtB->Va_k = _rtB->Va_f;

14709


14710
      /* Gain: '<S449>/Gain1' */

14711
      _rtB->Gain1_m = 1000.0 * _rtB->thetaL;

14712


14713
      /* DataTypeConversion: '<S449>/Data Type Conversion1' */

14714
      TrigonometricFunction4_o = floor(_rtB->Gain1_m);

14715
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14716
      {

14717
        TrigonometricFunction4_o = 0.0;

14718
      } else {

14719
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14720
      }

14721


14722
      _rtB->thetaL_e = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14723
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14724


14725
      /* End of DataTypeConversion: '<S449>/Data Type Conversion1' */

14726
      _rtB->thetaL_d = _rtB->thetaL_e;

14727


14728
      /* Gain: '<S458>/Unit Conversion' */

14729
      _rtB->Gain2_ij = 57.295779513082323 * _rtB->UnitConversion;

14730


14731
      /* Gain: '<S449>/Gain2' */

14732
      _rtB->Gain2_ij *= 1000.0;

14733


14734
      /* DataTypeConversion: '<S449>/Data Type Conversion2' */

14735
      TrigonometricFunction4_o = floor(_rtB->Gain2_ij);

14736
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14737
      {

14738
        TrigonometricFunction4_o = 0.0;

14739
      } else {

14740
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14741
      }

14742


14743
      _rtB->psiL_j1 = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14744
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14745


14746
      /* End of DataTypeConversion: '<S449>/Data Type Conversion2' */

14747
      _rtB->psiL_j = _rtB->psiL_j1;

14748


14749
      /* Gain: '<S449>/Gain3' */

14750
      _rtB->Gain3_n = 1000.0 * _rtB->p;

14751


14752
      /* DataTypeConversion: '<S449>/Data Type Conversion3' */

14753
      TrigonometricFunction4_o = floor(_rtB->Gain3_n);

14754
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14755
      {

14756
        TrigonometricFunction4_o = 0.0;

14757
      } else {

14758
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14759
      }

14760


14761
      _rtB->p_i = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14762
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14763


14764
      /* End of DataTypeConversion: '<S449>/Data Type Conversion3' */

14765
      _rtB->p_pm = _rtB->p_i;

14766


14767
      /* Gain: '<S449>/Gain4' */

14768
      _rtB->Gain4_e = 1000.0 * _rtB->q;

14769


14770
      /* DataTypeConversion: '<S449>/Data Type Conversion4' */

14771
      TrigonometricFunction4_o = floor(_rtB->Gain4_e);

14772
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14773
      {

14774
        TrigonometricFunction4_o = 0.0;

14775
      } else {

14776
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14777
      }

14778


14779
      _rtB->q_i = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14780
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14781


14782
      /* End of DataTypeConversion: '<S449>/Data Type Conversion4' */

14783
      _rtB->q_je = _rtB->q_i;

14784


14785
      /* Gain: '<S449>/Gain5' */

14786
      _rtB->Gain5_l = 1000.0 * _rtB->r;

14787


14788
      /* DataTypeConversion: '<S449>/Data Type Conversion5' */

14789
      TrigonometricFunction4_o = floor(_rtB->Gain5_l);

14790
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14791
      {

14792
        TrigonometricFunction4_o = 0.0;

14793
      } else {

14794
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14795
      }

14796


14797
      _rtB->r_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14798
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14799


14800
      /* End of DataTypeConversion: '<S449>/Data Type Conversion5' */

14801
      _rtB->r_l = _rtB->r_a;

14802


14803
      /* Gain: '<S449>/Gain6' */

14804
      _rtB->Gain6_l = 1000.0 * _rtB->Abx;

14805


14806
      /* DataTypeConversion: '<S449>/Data Type Conversion6' */

14807
      TrigonometricFunction4_o = floor(_rtB->Gain6_l);

14808
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14809
      {

14810
        TrigonometricFunction4_o = 0.0;

14811
      } else {

14812
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14813
      }

14814


14815
      _rtB->Abx_e = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14816
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14817


14818
      /* End of DataTypeConversion: '<S449>/Data Type Conversion6' */

14819
      _rtB->Abx_o = _rtB->Abx_e;

14820


14821
      /* Gain: '<S449>/Gain7' */

14822
      _rtB->Gain7_n = 1000.0 * _rtB->Aby;

14823


14824
      /* DataTypeConversion: '<S449>/Data Type Conversion7' */

14825
      TrigonometricFunction4_o = floor(_rtB->Gain7_n);

14826
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14827
      {

14828
        TrigonometricFunction4_o = 0.0;

14829
      } else {

14830
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14831
      }

14832


14833
      _rtB->Aby_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14834
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14835


14836
      /* End of DataTypeConversion: '<S449>/Data Type Conversion7' */

14837
      _rtB->Aby_a = _rtB->Aby_m;

14838


14839
      /* Gain: '<S449>/Gain8' */

14840
      _rtB->Gain8_g = 1000.0 * _rtB->Abz;

14841


14842
      /* DataTypeConversion: '<S449>/Data Type Conversion8' */

14843
      TrigonometricFunction4_o = floor(_rtB->Gain8_g);

14844
      if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

14845
      {

14846
        TrigonometricFunction4_o = 0.0;

14847
      } else {

14848
        TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);

14849
      }

14850


14851
      _rtB->Abz_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)

14852
        -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;

14853


14854
      /* End of DataTypeConversion: '<S449>/Data Type Conversion8' */

14855
      _rtB->Abz_h = _rtB->Abz_a;

14856
    }

14857


14858
    /* End of Switch: '<S445>/Switch' */

14859


14860
    /* DataTypeConversion: '<S13>/Data Type Conversion24' */

14861
    _rtB->Switch_dn = _rtB->Lon_m;

14862


14863
    /* Gain: '<S13>/Gain16' */

14864
    _rtB->Lon = 1.0E-7 * _rtB->Switch_dn;

14865


14866
    /* DataTypeConversion: '<S13>/Data Type Conversion23' */

14867
    _rtB->Switch_dn = _rtB->Lat_pm;

14868


14869
    /* Gain: '<S13>/Gain15' */

14870
    _rtB->Lat = 1.0E-7 * _rtB->Switch_dn;

14871


14872
    /* DataTypeConversion: '<S13>/Data Type Conversion25' */

14873
    _rtB->Switch_dn = _rtB->Alt_o3;

14874


14875
    /* Gain: '<S13>/Gain17' */

14876
    _rtB->Altitude = 0.001 * _rtB->Switch_dn;

14877


14878
    /* MATLAB Function: '<S9>/MATLAB Function2' */

14879
    _rtB->Alt_o = _rtB->Altitude;

14880


14881
    /* MATLAB Function 'AFCS_MODEL1/PlanningSystem_LLA/MATLAB Function2': '<S334>:1' */

14882
    /* '<S334>:1:16' */

14883
    /* '<S334>:1:12' */

14884
    /* '<S334>:1:1' */

14885
    /* '<S334>:1:3' */

14886
    /* '<S334>:1:4' */

14887
    /* '<S334>:1:5' */

14888
    /* '<S334>:1:7' */

14889
    /* '<S334>:1:8' */

14890
    /* '<S334>:1:9' */

14891
    /* '<S334>:1:10' */

14892
    /* '<S334>:1:11' */

14893
    /* '<S334>:1:14' */

14894
    /* '<S334>:1:17' */

14895
    /* '<S334>:1:18' */

14896
    /* '<S334>:1:19' */

14897
    /* '<S334>:1:20' */

14898
    /* '<S334>:1:21' */

14899
    /* '<S334>:1:22' */

14900
    /* '<S334>:1:23' */

14901
    /* '<S334>:1:24' */

14902
    /* '<S334>:1:25' */

14903
    d_0 = -90.0 - _rtB->LLA0[0];

14904
    AFCS_MODEL1_cosd_i(&d_0);

14905
    e_0 = 90.0 + _rtB->LLA0[0];

14906
    AFCS_MODEL1_sind_k(&e_0);

14907
    f_0 = -90.0 - _rtB->LLA0[0];

14908
    AFCS_MODEL1_sind_k(&f_0);

14909
    g_0 = -90.0 - _rtB->LLA0[0];

14910
    AFCS_MODEL1_cosd_i(&g_0);

14911
    h_0 = _rtB->LLA0[1];

14912
    AFCS_MODEL1_cosd_i(&h_0);

14913
    i = _rtB->LLA0[1];

14914
    AFCS_MODEL1_sind_k(&i);

14915
    j_0 = -_rtB->LLA0[1];

14916
    AFCS_MODEL1_sind_k(&j_0);

14917
    k_0 = _rtB->LLA0[1];

14918
    AFCS_MODEL1_cosd_i(&k_0);

14919
    l_0 = _rtB->Lat;

14920
    AFCS_MODEL1_cosd_i(&l_0);

14921
    m_0 = _rtB->Lon;

14922
    AFCS_MODEL1_cosd_i(&m_0);

14923
    n = _rtB->LLA0[0];

14924
    AFCS_MODEL1_cosd_i(&n);

14925
    o = _rtB->LLA0[1];

14926
    AFCS_MODEL1_cosd_i(&o);

14927
    p = _rtB->Lat;

14928
    AFCS_MODEL1_cosd_i(&p);

14929
    q = _rtB->Lon;

14930
    AFCS_MODEL1_sind_k(&q);

14931
    TrigonometricFunction4_o = _rtB->LLA0[0];

14932
    AFCS_MODEL1_cosd_i(&TrigonometricFunction4_o);

14933
    T = _rtB->LLA0[1];

14934
    AFCS_MODEL1_sind_k(&T);

14935
    tmpForInput_idx_1 = _rtB->Lat;

14936
    AFCS_MODEL1_sind_k(&tmpForInput_idx_1);

14937
    tmpForInput_idx_2 = _rtB->LLA0[0];

14938
    AFCS_MODEL1_sind_k(&tmpForInput_idx_2);

14939
    d_2[0] = d_0;

14940
    d_2[3] = 0.0;

14941
    d_2[6] = e_0;

14942
    d_2[1] = 0.0;

14943
    d_2[4] = 1.0;

14944
    d_2[7] = 0.0;

14945
    d_2[2] = f_0;

14946
    d_2[5] = 0.0;

14947
    d_2[8] = g_0;

14948
    h_2[0] = h_0;

14949
    h_2[3] = i;

14950
    h_2[6] = 0.0;

14951
    h_2[1] = j_0;

14952
    h_2[4] = k_0;

14953
    h_2[7] = 0.0;

14954
    h_2[2] = 0.0;

14955
    h_2[5] = 0.0;

14956
    h_2[8] = 1.0;

14957
    for (i_1 = 0; i_1 < 3; i_1++) {

14958
      for (flag = 0; flag < 3; flag++) {

14959
        j_2[i_1 + 3 * flag] = 0.0;

14960
        j_2[i_1 + 3 * flag] += h_2[3 * flag] * d_2[i_1];

14961
        j_2[i_1 + 3 * flag] += h_2[3 * flag + 1] * d_2[i_1 + 3];

14962
        j_2[i_1 + 3 * flag] += h_2[3 * flag + 2] * d_2[i_1 + 6];

14963
      }

14964
    }

14965


14966
    tmpForInput_idx_0 = (6.378137E+6 + _rtB->Alt_o) * l_0 * m_0 - (6.378137E+6 +

14967
      _rtB->LLA0[2]) * n * o;

14968
    TrigonometricFunction4_o = (6.378137E+6 + _rtB->Alt_o) * p * q -

14969
      (6.378137E+6 + _rtB->LLA0[2]) * TrigonometricFunction4_o * T;

14970
    T = (6.378137E+6 + _rtB->Alt_o) * tmpForInput_idx_1 - (6.378137E+6 +

14971
      _rtB->LLA0[2]) * tmpForInput_idx_2;

14972
    for (i_1 = 0; i_1 < 3; i_1++) {

14973
      Pos_NED[i_1] = j_2[i_1 + 6] * T + (j_2[i_1 + 3] * TrigonometricFunction4_o

14974
        + j_2[i_1] * tmpForInput_idx_0);

14975
    }

14976


14977
    /* '<S334>:1:27' */

14978
    _rtB->xn = Pos_NED[0];

14979


14980
    /* '<S334>:1:28' */

14981
    _rtB->ye = Pos_NED[1];

14982


14983
    /* '<S334>:1:29' */

14984
    _rtB->Alt_o -= _rtB->LLA0[2];

14985


14986
    /* End of MATLAB Function: '<S9>/MATLAB Function2' */

14987


14988
    /* Outport: '<Root>/X' */

14989
    ((real_T *)ssGetOutputPortSignal(S, 0))[0] = _rtB->xn;

14990


14991
    /* Outport: '<Root>/Y' */

14992
    ((real_T *)ssGetOutputPortSignal(S, 1))[0] = _rtB->ye;

14993


14994
    /* DataTypeConversion: '<S24>/Data Type Conversion45' */

14995
    _rtB->altitude = (real32_T)_rtB->Altitude;

14996


14997
    /* Outport: '<Root>/Hp' */

14998
    ((real32_T *)ssGetOutputPortSignal(S, 2))[0] = _rtB->altitude;

14999


15000
    /* Sum: '<S293>/Sum' */

15001
    _rtB->mh_j = _rtB->altitude;

15002


15003
    /* Outport: '<Root>/HBC' */

15004
    ((real32_T *)ssGetOutputPortSignal(S, 3))[0] = _rtB->mh_j;

15005


15006
    /* Outport: '<Root>/Hg' */

15007
    ((real32_T *)ssGetOutputPortSignal(S, 4))[0] = _rtB->mh_j;

15008


15009
    /* DataTypeConversion: '<S13>/Data Type Conversion26' */

15010
    _rtB->Switch_dn = _rtB->Vn;

15011


15012
    /* Gain: '<S13>/Gain12' */

15013
    _rtB->GPS_vnorth = 0.001 * _rtB->Switch_dn;

15014


15015
    /* DataTypeConversion: '<S24>/Data Type Conversion42' */

15016
    _rtB->vnorth = (real32_T)_rtB->GPS_vnorth;

15017


15018
    /* Outport: '<Root>/Vx' */

15019
    ((real32_T *)ssGetOutputPortSignal(S, 5))[0] = _rtB->vnorth;

15020


15021
    /* DataTypeConversion: '<S13>/Data Type Conversion27' */

15022
    _rtB->Switch_dn = _rtB->Ve;

15023


15024
    /* Gain: '<S13>/Gain13' */

15025
    _rtB->GPS_veast = 0.001 * _rtB->Switch_dn;

15026


15027
    /* DataTypeConversion: '<S24>/Data Type Conversion43' */

15028
    _rtB->veast = (real32_T)_rtB->GPS_veast;

15029


15030
    /* Outport: '<Root>/Vy' */

15031
    ((real32_T *)ssGetOutputPortSignal(S, 6))[0] = _rtB->veast;

15032


15033
    /* DataTypeConversion: '<S13>/Data Type Conversion28' */

15034
    _rtB->Switch_dn = _rtB->Vd;

15035


15036
    /* Gain: '<S13>/Gain14' */

15037
    _rtB->GPS_vdown = 0.001 * _rtB->Switch_dn;

15038


15039
    /* Gain: '<S24>/Gain4' */

15040
    _rtB->Switch_dn = -_rtB->GPS_vdown;

15041


15042
    /* DataTypeConversion: '<S24>/Data Type Conversion44' */

15043
    _rtB->mhdot = (real32_T)_rtB->Switch_dn;

15044


15045
    /* Outport: '<Root>/Vh' */

15046
    ((real32_T *)ssGetOutputPortSignal(S, 7))[0] = _rtB->mhdot;

15047


15048
    /* DataTypeConversion: '<S13>/Data Type Conversion30' */

15049
    _rtB->Switch_dn = _rtB->Ndefine1;

15050


15051
    /* Gain: '<S13>/Gain19' */

15052
    _rtB->Switch_dn *= 0.001;

15053


15054
    /* DataTypeConversion: '<S24>/Data Type Conversion9' */

15055
    _rtB->Alfa_j = (real32_T)_rtB->Switch_dn;

15056


15057
    /* Outport: '<Root>/Alpha' */

15058
    ((real32_T *)ssGetOutputPortSignal(S, 8))[0] = _rtB->Alfa_j;

15059


15060
    /* DataTypeConversion: '<S13>/Data Type Conversion31' */

15061
    _rtB->Switch_dn = _rtB->Ndefine2;

15062


15063
    /* Gain: '<S13>/Gain20' */

15064
    _rtB->Switch_dn *= 0.001;

15065


15066
    /* DataTypeConversion: '<S24>/Data Type Conversion10' */

15067
    _rtB->Beta_f = (real32_T)_rtB->Switch_dn;

15068


15069
    /* Outport: '<Root>/Beta' */

15070
    ((real32_T *)ssGetOutputPortSignal(S, 9))[0] = _rtB->Beta_f;

15071


15072
    /* DataTypeConversion: '<S13>/Data Type Conversion15' */

15073
    _rtB->Switch_dn = _rtB->thetaL_d;

15074


15075
    /* Gain: '<S13>/Gain3' */

15076
    _rtB->Switch_dn *= 0.001;

15077


15078
    /* DataTypeConversion: '<S24>/Data Type Conversion37' */

15079
    _rtB->thetaL_k = (real32_T)_rtB->Switch_dn;

15080


15081
    /* Outport: '<Root>/Theta' */

15082
    ((real32_T *)ssGetOutputPortSignal(S, 10))[0] = _rtB->thetaL_k;

15083


15084
    /* DataTypeConversion: '<S13>/Data Type Conversion14' */

15085
    _rtB->Switch_dn = _rtB->phiL_j;

15086


15087
    /* Gain: '<S13>/Gain9' */

15088
    _rtB->Switch_dn *= 0.001;

15089


15090
    /* DataTypeConversion: '<S24>/Data Type Conversion38' */

15091
    _rtB->phiL_n = (real32_T)_rtB->Switch_dn;

15092


15093
    /* Outport: '<Root>/Phi' */

15094
    ((real32_T *)ssGetOutputPortSignal(S, 11))[0] = _rtB->phiL_n;

15095


15096
    /* DataTypeConversion: '<S13>/Data Type Conversion16' */

15097
    _rtB->Switch_dn = _rtB->psiL_j;

15098


15099
    /* Gain: '<S13>/Gain4' */

15100
    _rtB->Switch_dn *= 0.001;

15101


15102
    /* DataTypeConversion: '<S24>/Data Type Conversion36' */

15103
    _rtB->psiL_d = (real32_T)_rtB->Switch_dn;

15104


15105
    /* Outport: '<Root>/Psi' */

15106
    ((real32_T *)ssGetOutputPortSignal(S, 12))[0] = _rtB->psiL_d;

15107


15108
    /* Trigonometry: '<S1>/Trigonometric Function2' */

15109
    _rtB->Sqrt = AFCS_MODEL1_rt_atan2f_snf(_rtB->veast, _rtB->vnorth);

15110


15111
    /* Gain: '<S3>/Unit Conversion' */

15112
    _rtB->Sqrt *= 57.2957802F;

15113


15114
    /* Outport: '<Root>/TRK' */

15115
    ((real32_T *)ssGetOutputPortSignal(S, 13))[0] = _rtB->Sqrt;

15116


15117
    /* DataTypeConversion: '<S13>/Data Type Conversion17' */

15118
    _rtB->Switch_dn = _rtB->p_pm;

15119


15120
    /* Gain: '<S13>/Gain5' */

15121
    _rtB->Switch_dn *= 0.001;

15122


15123
    /* DataTypeConversion: '<S24>/Data Type Conversion33' */

15124
    _rtB->p_c = (real32_T)_rtB->Switch_dn;

15125


15126
    /* Outport: '<Root>/p' */

15127
    ((real32_T *)ssGetOutputPortSignal(S, 14))[0] = _rtB->p_c;

15128


15129
    /* DataTypeConversion: '<S13>/Data Type Conversion18' */

15130
    _rtB->Switch_dn = _rtB->q_je;

15131


15132
    /* Gain: '<S13>/Gain6' */

15133
    _rtB->Switch_dn *= 0.001;

15134


15135
    /* DataTypeConversion: '<S24>/Data Type Conversion34' */

15136
    _rtB->q_k = (real32_T)_rtB->Switch_dn;

15137


15138
    /* Outport: '<Root>/q' */

15139
    ((real32_T *)ssGetOutputPortSignal(S, 15))[0] = _rtB->q_k;

15140


15141
    /* DataTypeConversion: '<S13>/Data Type Conversion19' */

15142
    _rtB->Switch_dn = _rtB->r_l;

15143


15144
    /* Gain: '<S13>/Gain7' */

15145
    _rtB->Switch_dn *= 0.001;

15146


15147
    /* DataTypeConversion: '<S24>/Data Type Conversion35' */

15148
    _rtB->r_n = (real32_T)_rtB->Switch_dn;

15149


15150
    /* Outport: '<Root>/r' */

15151
    ((real32_T *)ssGetOutputPortSignal(S, 16))[0] = _rtB->r_n;

15152


15153
    /* DataTypeConversion: '<S13>/Data Type Conversion20' */

15154
    _rtB->Switch_dn = _rtB->Abx_o;

15155


15156
    /* Gain: '<S13>/Gain8' */

15157
    _rtB->Switch_dn *= 0.001;

15158


15159
    /* Gain: '<S24>/Gain' */

15160
    _rtB->Switch_dn *= 0.1019367991845056;

15161


15162
    /* DataTypeConversion: '<S24>/Data Type Conversion39' */

15163
    _rtB->Nx = (real32_T)_rtB->Switch_dn;

15164


15165
    /* Outport: '<Root>/Ax' */

15166
    ((real32_T *)ssGetOutputPortSignal(S, 17))[0] = _rtB->Nx;

15167


15168
    /* DataTypeConversion: '<S13>/Data Type Conversion21' */

15169
    _rtB->Switch_dn = _rtB->Aby_a;

15170


15171
    /* Gain: '<S13>/Gain10' */

15172
    _rtB->Switch_dn *= 0.001;

15173


15174
    /* Gain: '<S24>/Gain1' */

15175
    _rtB->Switch_dn *= 0.1019367991845056;

15176


15177
    /* DataTypeConversion: '<S24>/Data Type Conversion40' */

15178
    _rtB->Ny = (real32_T)_rtB->Switch_dn;

15179


15180
    /* Outport: '<Root>/Ay' */

15181
    ((real32_T *)ssGetOutputPortSignal(S, 18))[0] = _rtB->Ny;

15182


15183
    /* DataTypeConversion: '<S13>/Data Type Conversion22' */

15184
    _rtB->Switch_dn = _rtB->Abz_h;

15185


15186
    /* Gain: '<S13>/Gain11' */

15187
    _rtB->Switch_dn *= 0.001;

15188


15189
    /* Gain: '<S24>/Gain2' */

15190
    _rtB->Switch_dn *= 0.1019367991845056;

15191


15192
    /* DataTypeConversion: '<S24>/Data Type Conversion41' */

15193
    _rtB->Nz = (real32_T)_rtB->Switch_dn;

15194


15195
    /* Outport: '<Root>/Az' */

15196
    ((real32_T *)ssGetOutputPortSignal(S, 19))[0] = _rtB->Nz;

15197


15198
    /* Product: '<S24>/Product' */

15199
    _rtB->Sqrt = _rtB->vnorth * _rtB->vnorth;

15200


15201
    /* Product: '<S24>/Product1' */

15202
    _rtB->ascmd_n = _rtB->veast * _rtB->veast;

15203


15204
    /* Product: '<S24>/Product2' */

15205
    _rtB->Gain_e = _rtB->mhdot * _rtB->mhdot;

15206


15207
    /* Sum: '<S24>/Sum' */

15208
    _rtB->Sqrt = (_rtB->Sqrt + _rtB->ascmd_n) + _rtB->Gain_e;

15209


15210
    /* Sqrt: '<S24>/Sqrt' */

15211
    _rtB->Sqrt = (real32_T)sqrt(_rtB->Sqrt);

15212


15213
    /* DataTypeConversion: '<S24>/Data Type Conversion32' */

15214
    _rtB->vt = _rtB->Sqrt;

15215
  }

15216


15217
  /* End of Outputs for SubSystem: '<S1>/Subsystem5' */

15218
  if (ssIsSampleHit(S, 1, 0)) {

15219
    /* UnitDelay: '<S24>/Unit Delay3' */

15220
    _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 352))[0];

15221


15222
    /* RelationalOperator: '<S24>/Relational Operator1' incorporates:

15223
     *  Constant: '<S24>/Constant1'

15224
     */

15225
    _rtB->RelationalOperator19 = (0 == _rtB->UnitDelay5_b);

15226


15227
    /* RelationalOperator: '<S24>/Relational Operator3' incorporates:

15228
     *  Constant: '<S24>/Constant7'

15229
     */

15230
    _rtB->RelationalOperator12 = (_rtB->UnitDelay5_b == 1);

15231


15232
    /* RelationalOperator: '<S24>/Relational Operator2' incorporates:

15233
     *  Constant: '<S24>/Constant2'

15234
     */

15235
    _rtB->RelationalOperator2 = (_rtB->UnitDelay5_b == 6);

15236


15237
    /* Logic: '<S24>/Logical Operator' */

15238
    _rtB->LogicalOperator = (_rtB->RelationalOperator19 ||

15239
      _rtB->RelationalOperator12 || _rtB->RelationalOperator2);

15240


15241
    /* DataTypeConversion: '<S17>/Data Type Conversion1' */

15242
    memset(&_rtB->routeFG[0], 0, 30U * sizeof(int32_T));

15243
  }

15244


15245
  if (1) {

15246
    /* RelationalOperator: '<S24>/Relational Operator' incorporates:

15247
     *  Constant: '<S24>/Constant3'

15248
     */

15249
    _rtB->LogicalOperator1_b = (_rtB->vt < 0.3);

15250


15251
    /* Logic: '<S24>/Logical Operator1' */

15252
    _rtB->LogicalOperator1_b = (_rtB->LogicalOperator &&

15253
      _rtB->LogicalOperator1_b);

15254


15255
    /* Step: '<S17>/Step1' */

15256
    _rtB->Switch_dn = !(ssGetTaskTime(S,0) < 1.0);

15257


15258
    /* Sum: '<S17>/Sum' incorporates:

15259
     *  Constant: '<S17>/Constant14'

15260
     */

15261
    _rtB->UnaryMinus = 1.0 - _rtB->Switch_dn;

15262


15263
    /* DataTypeConversion: '<S17>/Data Type Conversion2' */

15264
    TrigonometricFunction4_o = floor(_rtB->UnaryMinus);

15265
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

15266
    {

15267
      TrigonometricFunction4_o = 0.0;

15268
    } else {

15269
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);

15270
    }

15271


15272
    _rtB->airLineUpdateFlag = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)

15273
      (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)

15274
      (uint8_T)TrigonometricFunction4_o);

15275


15276
    /* End of DataTypeConversion: '<S17>/Data Type Conversion2' */

15277


15278
    /* RelationalOperator: '<S301>/Compare' incorporates:

15279
     *  Constant: '<S301>/Constant'

15280
     */

15281
    _rtB->Compare_c = (_rtB->airLineUpdateFlag == 1);

15282
  }

15283


15284
  if (ssIsSampleHit(S, 1, 0)) {

15285
    /* SignalConversion: '<S299>/HiddenBuf_InsertedFor_Enabled Subsystem_at_inport_3' */

15286
    _rtB->HiddenBuf_InsertedFor_EnabledSu = _rtB->Compare_c;

15287


15288
    /* Outputs for Enabled SubSystem: '<S299>/Enabled Subsystem' incorporates:

15289
     *  EnablePort: '<S302>/Enable'

15290
     */

15291
    if (ssIsMajorTimeStep(S)) {

15292
      if (_rtB->HiddenBuf_InsertedFor_EnabledSu) {

15293
        if (!((boolean_T *)ssGetDWork(S, 613))[0]) {

15294
          /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:

15295
           *  EnablePort: '<S651>/Enable'

15296
           */

15297
          if (ssGetTaskTime(S,1) != ssGetTStart(S)) {

15298
            ssSetSolverNeedsReset(S);

15299
          }

15300


15301
          /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */

15302
          ((boolean_T *)ssGetDWork(S, 613))[0] = true;

15303
        }

15304
      } else {

15305
        if (((boolean_T *)ssGetDWork(S, 613))[0]) {

15306
          ssSetSolverNeedsReset(S);

15307
          ((boolean_T *)ssGetDWork(S, 613))[0] = false;

15308
        }

15309
      }

15310
    }

15311


15312
    /* End of Outputs for SubSystem: '<S299>/Enabled Subsystem' */

15313


15314
    /* UnitDelay: '<S7>/Unit Delay' */

15315
    _rtB->UnitDelay = ((int8_T *)ssGetDWork(S, 353))[0];

15316
  }

15317


15318
  /* Outputs for Enabled SubSystem: '<S299>/Enabled Subsystem' incorporates:

15319
   *  EnablePort: '<S302>/Enable'

15320
   */

15321
  if (((boolean_T *)ssGetDWork(S, 613))[0] && 1) {

15322
    /* Inport: '<S302>/airNumPoint' */

15323
    _rtB->airNumPoint = _rtB->NumPoint;

15324


15325
    /* MATLAB Function: '<S302>/MATLAB Function' */

15326
    /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/Enabled Subsystem/MATLAB Function': '<S303>:1' */

15327
    /* '<S303>:1:5' */

15328
    memset(&_rtB->A_b[0], 0, 120U * sizeof(real_T));

15329


15330
    /* '<S303>:1:7' */

15331
    for (rtAction = 1; rtAction <= _rtB->airNumPoint; rtAction++) {

15332
      /* '<S303>:1:7' */

15333
      /* '<S303>:1:9' */

15334
      i_1 = rtAction - 1;

15335
      _rtB->A_b[i_1] = _rtB->route_Lon[rtAction - 1];

15336
      _rtB->A_b[30 + i_1] = _rtB->route_Lat[rtAction - 1];

15337
      _rtB->A_b[60 + i_1] = _rtB->routeAlt[rtAction - 1];

15338
      _rtB->A_b[90 + i_1] = _rtB->routeFG[rtAction - 1];

15339
    }

15340


15341
    /* End of MATLAB Function: '<S302>/MATLAB Function' */

15342


15343
    /* SignalConversion: '<S302>/OutportBufferForA' */

15344
    memcpy(&_rtB->OutportBufferForA[0], &_rtB->A_b[0], 120U * sizeof(real_T));

15345
  }

15346


15347
  /* End of Outputs for SubSystem: '<S299>/Enabled Subsystem' */

15348
  if (ssIsSampleHit(S, 1, 0)) {

15349
    /* UnitDelay: '<S7>/Unit Delay3' */

15350
    _rtB->UnitDelay3 = ((int8_T *)ssGetDWork(S, 354))[0];

15351
  }

15352


15353
  if (1) {

15354
    /* MATLAB Function: '<S293>/MATLAB Function3' */

15355
    rtAction = _rtB->UnitDelay;

15356


15357
    /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/MATLAB Function3': '<S298>:1' */

15358
    if (_rtB->UnitDelay <= 0) {

15359
      /* '<S298>:1:3' */

15360
      /* '<S298>:1:4' */

15361
      rtAction = 1;

15362
    }

15363


15364
    /* '<S298>:1:9' */

15365
    i_1 = rtAction + 1;

15366
    if (i_1 > 127) {

15367
      i_1 = 127;

15368
    }

15369


15370
    /* '<S298>:1:11' */

15371
    /* '<S298>:1:12' */

15372
    /*  Vxd=int32(A(kk,4)); */

15373
    /* '<S298>:1:16' */

15374
    /* '<S298>:1:17' */

15375
    /* '<S298>:1:18' */

15376
    /*  Vxd1=int32(A(kk1,4)); */

15377
    /* '<S298>:1:20' */

15378
    flag = _rtB->airNumPoint - 3;

15379
    if (flag < -128) {

15380
      flag = -128;

15381
    }

15382


15383
    _rtB->Vxd1 = flag;

15384


15385
    /* 当k为倒数第4个航点时，就要切到进场模式了 */

15386
    /* %%%%%%%%%%%%%%%%%%%%% */

15387
    /* '<S298>:1:23' */

15388
    /* '<S298>:1:24' */

15389
    /* '<S298>:1:25' */

15390
    /* '<S298>:1:26' */

15391
    /* '<S298>:1:27' */

15392
    AFCS_MODEL1_GetPsi(_rtB->OutportBufferForA[rtAction - 1],

15393
                       _rtB->OutportBufferForA[rtAction + 29],

15394
                       _rtB->OutportBufferForA[(int8_T)i_1 - 1],

15395
                       _rtB->OutportBufferForA[(int8_T)i_1 + 29], &DD0, &PSDref0);

15396


15397
    /* '<S298>:1:28' */

15398
    /* '<S298>:1:29' */

15399
    /* '<S298>:1:30' */

15400
    _rtB->Href = (real32_T)_rtB->OutportBufferForA[(int8_T)i_1 + 59];

15401


15402
    /* %%%%%%%%%%%%%%%%%%%%% */

15403
    /* '<S298>:1:32' */

15404
    /* '<S298>:1:33' */

15405
    /* '<S298>:1:34' */

15406
    AFCS_MODEL1_GetPsi(_rtB->OutportBufferForA[rtAction - 1],

15407
                       _rtB->OutportBufferForA[rtAction + 29], _rtB->Lon,

15408
                       _rtB->Lat, &Dp0, &PSDp0);

15409


15410
    /* '<S298>:1:35' */

15411
    /* '<S298>:1:36' */

15412
    /* '<S298>:1:37' */

15413
    DXp = -(real32_T)Dp0 * (real32_T)cos((real32_T)PSDp0);

15414


15415
    /* '<S298>:1:38' */

15416
    DYp = -(real32_T)Dp0 * (real32_T)sin((real32_T)PSDp0);

15417


15418
    /* '<S298>:1:40' */

15419
    COSA = (real32_T)cos((real32_T)PSDref0);

15420


15421
    /* '<S298>:1:41' */

15422
    COSB = (real32_T)sin((real32_T)PSDref0);

15423


15424
    /* '<S298>:1:42' */

15425
    _rtB->DtoStart_b = -DXp * COSA - DYp * COSB;

15426


15427
    /* '<S298>:1:43' */

15428
    _rtB->dXac = (real32_T)DD0 - (-DXp * COSA - DYp * COSB);

15429


15430
    /* '<S298>:1:44' */

15431
    _rtB->dYac = -DYp * COSA + DXp * COSB;

15432


15433
    /* '<S298>:1:45' */

15434
    _rtB->vxrw_j = -_rtB->vnorth * (real32_T)cos((real32_T)PSDref0) -

15435
      _rtB->veast * (real32_T)sin((real32_T)PSDref0);

15436


15437
    /* '<S298>:1:46' */

15438
    _rtB->vyrw_f = -_rtB->vnorth * (real32_T)sin((real32_T)PSDref0) +

15439
      _rtB->veast * (real32_T)cos((real32_T)PSDref0);

15440


15441
    /* '<S298>:1:48' */

15442
    i_1 = _rtB->airNumPoint - 2;

15443
    if (i_1 < -128) {

15444
      i_1 = -128;

15445
    }

15446


15447
    _rtB->HrefAL = (real32_T)_rtB->OutportBufferForA[i_1 + 59];

15448


15449
    /* %进场的参考高度 */

15450
    /* '<S298>:1:49' */

15451
    _rtB->PSDref_n = (real32_T)PSDref0 * 57.2957802F;

15452


15453
    /* End of MATLAB Function: '<S293>/MATLAB Function3' */

15454


15455
    /* MATLAB Function: '<S293>/MATLAB Function2' */

15456
    /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/MATLAB Function2': '<S297>:1' */

15457
    /*  persistent  InitFg IndexAL0; */

15458
    /*  if isempty(InitFg) */

15459
    /*      InitFg=int8(0); */

15460
    /*      IndexAL0=int8(1);     */

15461
    /*  end */

15462
    /*  if(InitFg==int8(0)) */

15463
    /*     for i=1:NumPoint */

15464
    /*       Vxd=int32(A(i,4)); */

15465
    /*       if(Vxd==int32(7000)) */

15466
    /*          IndexAL0=i; */

15467
    /*       end */

15468
    /*     end */

15469
    /*      InitFg=int8(1); */

15470
    /*  end */

15471
    /* '<S297>:1:17' */

15472
    i_1 = _rtB->airNumPoint - 2;

15473
    if (i_1 < -128) {

15474
      i_1 = -128;

15475
    }

15476


15477
    if (1 >= (int8_T)i_1) {

15478
      rtAction = 1;

15479
    } else {

15480
      rtAction = (int8_T)i_1;

15481
    }

15482


15483
    /* '<S297>:1:18' */

15484
    _rtB->LonN = _rtB->OutportBufferForA[rtAction - 1];

15485


15486
    /* '<S297>:1:19' */

15487
    _rtB->LatN = _rtB->OutportBufferForA[rtAction + 29];

15488


15489
    /* '<S297>:1:20' */

15490
    _rtB->LonN1 = _rtB->OutportBufferForA[rtAction];

15491


15492
    /* '<S297>:1:21' */

15493
    _rtB->LatN1 = _rtB->OutportBufferForA[30 + rtAction];

15494
    _rtB->IndexAL_k = rtAction;

15495


15496
    /* End of MATLAB Function: '<S293>/MATLAB Function2' */

15497


15498
    /* MATLAB Function: '<S293>/MATLAB Function1' */

15499
    /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/MATLAB Function1': '<S296>:1' */

15500
    /* %%%%%%%%%%%%%%%%%%%%% */

15501
    /* '<S296>:1:6' */

15502
    /* '<S296>:1:14' */

15503
    /* '<S296>:1:15' */

15504
    /* '<S296>:1:16' */

15505
    /* '<S296>:1:17' */

15506
    /* '<S296>:1:6' */

15507
    _rtB->Xnorth_j = (_rtB->Lat - _rtB->LatN) * 0.017453292519943295 * 6.371E+6;

15508


15509
    /* '<S296>:1:6' */

15510
    _rtB->Yeast_m = cos((_rtB->LatN + _rtB->Lat) * 0.5 * 0.017453292519943295) *

15511
      ((_rtB->Lon - _rtB->LonN) * 0.017453292519943295) * 6.371E+6;

15512


15513
    /* '<S296>:1:7' */

15514
    /* '<S296>:1:14' */

15515
    /* '<S296>:1:15' */

15516
    /* '<S296>:1:16' */

15517
    /* '<S296>:1:17' */

15518
    /* '<S296>:1:7' */

15519
    _rtB->XnorthD1 = (_rtB->LatN1 - _rtB->LatN) * 0.017453292519943295 *

15520
      6.371E+6;

15521


15522
    /* '<S296>:1:7' */

15523
    _rtB->YeastD1 = cos((_rtB->LatN + _rtB->LatN1) * 0.5 * 0.017453292519943295)

15524
      * ((_rtB->LonN1 - _rtB->LonN) * 0.017453292519943295) * 6.371E+6;

15525


15526
    /* '<S296>:1:8' */

15527
    _rtB->XnorthD = 0.0;

15528


15529
    /* '<S296>:1:9' */

15530
    _rtB->YeastD = 0.0;

15531


15532
    /* DataTypeConversion: '<S293>/Data Type Conversion21' */

15533
    _rtB->DataTypeConversion21 = (real32_T)_rtB->Xnorth_j;

15534


15535
    /* DataTypeConversion: '<S293>/Data Type Conversion20' */

15536
    _rtB->DataTypeConversion20 = (real32_T)_rtB->Yeast_m;

15537


15538
    /* DataTypeConversion: '<S293>/Data Type Conversion9' */

15539
    _rtB->DataTypeConversion9 = (real32_T)_rtB->XnorthD;

15540


15541
    /* DataTypeConversion: '<S293>/Data Type Conversion12' */

15542
    _rtB->DataTypeConversion12 = (real32_T)_rtB->YeastD;

15543


15544
    /* DataTypeConversion: '<S293>/Data Type Conversion13' */

15545
    _rtB->DataTypeConversion13 = (real32_T)_rtB->XnorthD1;

15546


15547
    /* DataTypeConversion: '<S293>/Data Type Conversion14' */

15548
    _rtB->DataTypeConversion14 = (real32_T)_rtB->YeastD1;

15549


15550
    /* MATLAB Function: '<S293>/CompTP' incorporates:

15551
     *  Constant: '<S17>/Constant13'

15552
     */

15553
    /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/CompTP': '<S295>:1' */

15554
    /* Computing Tangent Points; */

15555
    /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算冻结航向 */

15556
    if (_rtB->UnitDelay3 > ((int8_T *)ssGetDWork(S, 516))[0]) {

15557
      /* '<S295>:1:16' */

15558
      /* '<S295>:1:17' */

15559
      ((real32_T *)ssGetDWork(S, 287))[0] = _rtB->DataTypeConversion21;

15560


15561
      /* '<S295>:1:18' */

15562
      ((real32_T *)ssGetDWork(S, 288))[0] = _rtB->DataTypeConversion20;

15563
    }

15564


15565
    /* '<S295>:1:20' */

15566
    ((int8_T *)ssGetDWork(S, 516))[0] = _rtB->UnitDelay3;

15567


15568
    /* %%%%%%%%%% */

15569
    /* '<S295>:1:23' */

15570
    AFCS_MODEL1_CompDistance(_rtB->DataTypeConversion9,

15571
      _rtB->DataTypeConversion12, _rtB->DataTypeConversion13,

15572
      _rtB->DataTypeConversion14, ((real32_T *)ssGetDWork(S, 287))[0],

15573
      ((real32_T *)ssGetDWork(S, 288))[0], &DD, &PsdNorthD, &Xrw, &Yrw);

15574
    if (Yrw < 0.0F) {

15575
      /* '<S295>:1:24' */

15576
      /* '<S295>:1:25' */

15577
      DireFg = -1;

15578
    } else {

15579
      /* '<S295>:1:28' */

15580
      DireFg = 1;

15581
    }

15582


15583
    /* %%%%%%%% */

15584
    /* %%% */

15585
    /* '<S295>:1:33' */

15586
    b = PsdNorthD - 90.0F;

15587
    AFCS_MODEL1_cosd(&b);

15588
    Xcircl1 = 250.0F * b + _rtB->DataTypeConversion9;

15589


15590
    /* '<S295>:1:34' */

15591
    c = PsdNorthD - 90.0F;

15592
    AFCS_MODEL1_sind(&c);

15593
    Ycircl1 = 250.0F * c + _rtB->DataTypeConversion12;

15594


15595
    /* %%%  */

15596
    /* '<S295>:1:36' */

15597
    d_1 = PsdNorthD + 90.0F;

15598
    AFCS_MODEL1_cosd(&d_1);

15599
    Xcircl2 = 250.0F * d_1 + _rtB->DataTypeConversion9;

15600


15601
    /* '<S295>:1:37' */

15602
    e_1 = PsdNorthD + 90.0F;

15603
    AFCS_MODEL1_sind(&e_1);

15604
    Ycircl2 = 250.0F * e_1 + _rtB->DataTypeConversion12;

15605


15606
    /* %%%%%%%%% */

15607
    if (Yrw < 0.0F) {

15608
      /* '<S295>:1:39' */

15609
      /* '<S295>:1:40' */

15610
      DX = ((real32_T *)ssGetDWork(S, 287))[0] - Xcircl1;

15611


15612
      /* '<S295>:1:41' */

15613
      DY = ((real32_T *)ssGetDWork(S, 288))[0] - Ycircl1;

15614


15615
      /* '<S295>:1:42' */

15616
      DDOC = (real32_T)sqrt(DX * DX + DY * DY);

15617


15618
      /* '<S295>:1:43' */

15619
      PsiOC = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;

15620
    } else {

15621
      /* '<S295>:1:45' */

15622
      DX = ((real32_T *)ssGetDWork(S, 287))[0] - Xcircl2;

15623


15624
      /* '<S295>:1:46' */

15625
      DY = ((real32_T *)ssGetDWork(S, 288))[0] - Ycircl2;

15626


15627
      /* '<S295>:1:47' */

15628
      DDOC = (real32_T)sqrt(DX * DX + DY * DY);

15629


15630
      /* '<S295>:1:48' */

15631
      PsiOC = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;

15632
    }

15633


15634
    if (DDOC < 250.0F) {

15635
      /* '<S295>:1:51' */

15636
      /* %%%%%%%%% */

15637
      if (Yrw < 0.0F) {

15638
        /* '<S295>:1:53' */

15639
        /* '<S295>:1:54' */

15640
        DireFg = 1;

15641


15642
        /* '<S295>:1:55' */

15643
        DX = ((real32_T *)ssGetDWork(S, 287))[0] - Xcircl2;

15644


15645
        /* '<S295>:1:56' */

15646
        DY = ((real32_T *)ssGetDWork(S, 288))[0] - Ycircl2;

15647


15648
        /* '<S295>:1:57' */

15649
        /* '<S295>:1:58' */

15650
        PsiOC = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;

15651


15652
        /* '<S295>:1:60' */

15653
        DXp = 250.0F / (real32_T)sqrt(DX * DX + DY * DY);

15654
        if (!(DXp <= 1.0F)) {

15655
          DXp = 1.0F;

15656
        }

15657


15658
        Deta = 57.2957802F * (real32_T)acos(DXp);

15659


15660
        /* %%右切点 */

15661
        /* '<S295>:1:62' */

15662
        alp1 = PsiOC + Deta;

15663


15664
        /* %%左切点 */

15665
        /* '<S295>:1:64' */

15666
        alp2 = PsiOC - Deta;

15667


15668
        /* '<S295>:1:65' */

15669
        Xtp1 = alp1;

15670
        AFCS_MODEL1_cosd(&Xtp1);

15671
        Xtp1 = 250.0F * Xtp1 + Xcircl2;

15672


15673
        /* '<S295>:1:66' */

15674
        Ytp1 = alp1;

15675
        AFCS_MODEL1_sind(&Ytp1);

15676
        Ytp1 = 250.0F * Ytp1 + Ycircl2;

15677


15678
        /* '<S295>:1:67' */

15679
        Xtp2 = alp2;

15680
        AFCS_MODEL1_cosd(&Xtp2);

15681
        Xtp2 = 250.0F * Xtp2 + Xcircl2;

15682


15683
        /* '<S295>:1:68' */

15684
        DXp = alp2;

15685
        AFCS_MODEL1_sind(&DXp);

15686
        Ytp2 = 250.0F * DXp + Ycircl2;

15687
      } else {

15688
        /* '<S295>:1:72' */

15689
        DireFg = -1;

15690


15691
        /* '<S295>:1:73' */

15692
        DX = ((real32_T *)ssGetDWork(S, 287))[0] - Xcircl1;

15693


15694
        /* '<S295>:1:74' */

15695
        DY = ((real32_T *)ssGetDWork(S, 288))[0] - Ycircl1;

15696


15697
        /* '<S295>:1:75' */

15698
        /* '<S295>:1:76' */

15699
        PsiOC = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;

15700


15701
        /* '<S295>:1:78' */

15702
        DXp = 250.0F / (real32_T)sqrt(DX * DX + DY * DY);

15703
        if (!(DXp <= 1.0F)) {

15704
          DXp = 1.0F;

15705
        }

15706


15707
        Deta = 57.2957802F * (real32_T)acos(DXp);

15708


15709
        /* %%右切点 */

15710
        /* '<S295>:1:80' */

15711
        alp1 = PsiOC + Deta;

15712


15713
        /* %%左切点 */

15714
        /* '<S295>:1:82' */

15715
        alp2 = PsiOC - Deta;

15716


15717
        /* '<S295>:1:83' */

15718
        Xtp1 = alp1;

15719
        AFCS_MODEL1_cosd(&Xtp1);

15720
        Xtp1 = 250.0F * Xtp1 + Xcircl1;

15721


15722
        /* '<S295>:1:84' */

15723
        Ytp1 = alp1;

15724
        AFCS_MODEL1_sind(&Ytp1);

15725
        Ytp1 = 250.0F * Ytp1 + Ycircl1;

15726


15727
        /* '<S295>:1:85' */

15728
        Xtp2 = alp2;

15729
        AFCS_MODEL1_cosd(&Xtp2);

15730
        Xtp2 = 250.0F * Xtp2 + Xcircl1;

15731


15732
        /* '<S295>:1:86' */

15733
        DXp = alp2;

15734
        AFCS_MODEL1_sind(&DXp);

15735
        Ytp2 = 250.0F * DXp + Ycircl1;

15736
      }

15737
    } else {

15738
      /* %%%%%%%%% */

15739
      /* %%%% */

15740
      /* '<S295>:1:92' */

15741
      DXp = 250.0F / DDOC;

15742
      if (!(DXp <= 1.0F)) {

15743
        DXp = 1.0F;

15744
      }

15745


15746
      Deta = 57.2957802F * (real32_T)acos(DXp);

15747


15748
      /* %% */

15749
      /* '<S295>:1:94' */

15750
      alp1 = PsiOC + Deta;

15751


15752
      /* %% */

15753
      /* '<S295>:1:96' */

15754
      alp2 = PsiOC - Deta;

15755


15756
      /* %%%%% */

15757
      if (Yrw < 0.0F) {

15758
        /* '<S295>:1:99' */

15759
        /* '<S295>:1:100' */

15760
        g_1 = alp1;

15761
        AFCS_MODEL1_cosd(&g_1);

15762
        Xtp1 = 250.0F * g_1 + Xcircl1;

15763


15764
        /* '<S295>:1:101' */

15765
        i_0 = alp1;

15766
        AFCS_MODEL1_sind(&i_0);

15767
        Ytp1 = 250.0F * i_0 + Ycircl1;

15768


15769
        /* '<S295>:1:102' */

15770
        k_1 = alp2;

15771
        AFCS_MODEL1_cosd(&k_1);

15772
        Xtp2 = 250.0F * k_1 + Xcircl1;

15773


15774
        /* '<S295>:1:103' */

15775
        DXp = alp2;

15776
        AFCS_MODEL1_sind(&DXp);

15777
        Ytp2 = 250.0F * DXp + Ycircl1;

15778
      } else {

15779
        /* '<S295>:1:105' */

15780
        f_1 = alp1;

15781
        AFCS_MODEL1_cosd(&f_1);

15782
        Xtp1 = 250.0F * f_1 + Xcircl2;

15783


15784
        /* '<S295>:1:106' */

15785
        h_1 = alp1;

15786
        AFCS_MODEL1_sind(&h_1);

15787
        Ytp1 = 250.0F * h_1 + Ycircl2;

15788


15789
        /* '<S295>:1:107' */

15790
        j_1 = alp2;

15791
        AFCS_MODEL1_cosd(&j_1);

15792
        Xtp2 = 250.0F * j_1 + Xcircl2;

15793


15794
        /* '<S295>:1:108' */

15795
        DXp = alp2;

15796
        AFCS_MODEL1_sind(&DXp);

15797
        Ytp2 = 250.0F * DXp + Ycircl2;

15798
      }

15799
    }

15800


15801
    if (DireFg < 0) {

15802
      /* '<S295>:1:112' */

15803
      /* '<S295>:1:113' */

15804
      XnorthTP = Xtp2;

15805


15806
      /* '<S295>:1:114' */

15807
      YeastTP = Ytp2;

15808
    } else {

15809
      /* '<S295>:1:116' */

15810
      XnorthTP = Xtp1;

15811


15812
      /* '<S295>:1:117' */

15813
      YeastTP = Ytp1;

15814
    }

15815


15816
    /* '<S295>:1:120' */

15817
    /* '<S295>:1:121' */

15818
    /* '<S295>:1:122' */

15819
    PsdNorthRef = AFCS_MODEL1_rt_atan2f_snf(YeastTP - ((real32_T *)ssGetDWork(S,

15820
      288))[0], XnorthTP - ((real32_T *)ssGetDWork(S, 287))[0]) * 180.0F /

15821
      3.14159274F;

15822


15823
    /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算相对于新航线的侧偏和待飞距 */

15824
    /* '<S295>:1:125' */

15825
    AFCS_MODEL1_CompDistance(((real32_T *)ssGetDWork(S, 287))[0], ((real32_T *)

15826
      ssGetDWork(S, 288))[0], XnorthTP, YeastTP, _rtB->DataTypeConversion21,

15827
      _rtB->DataTypeConversion20, &b_DD, &b_PsdNorthD, &b_Xrw, &b_Yrw);

15828


15829
    /* '<S295>:1:125' */

15830
    _rtB->Xrw = b_Xrw;

15831


15832
    /* '<S295>:1:125' */

15833
    _rtB->Yrw = b_Yrw;

15834


15835
    /* '<S295>:1:126' */

15836
    _rtB->vxrw_a = (real32_T)cos(PsdNorthRef * 3.14159274F / 180.0F) *

15837
      -_rtB->vnorth - (real32_T)sin(PsdNorthRef * 3.14159274F / 180.0F) *

15838
      _rtB->veast;

15839


15840
    /* '<S295>:1:127' */

15841
    _rtB->vyrw_n = (real32_T)sin(PsdNorthRef * 3.14159274F / 180.0F) *

15842
      -_rtB->vnorth + (real32_T)cos(PsdNorthRef * 3.14159274F / 180.0F) *

15843
      _rtB->veast;

15844


15845
    /* %%%%%%计算当前位置与切入航点连线，与当前速度矢量夹角 */

15846
    /* '<S295>:1:129' */

15847
    /* '<S295>:1:130' */

15848
    /* '<S295>:1:131' */

15849
    /* '<S295>:1:132' */

15850
    /* '<S295>:1:133' */

15851
    _rtB->DetaPsi = AFCS_MODEL1_rt_atan2f_snf(_rtB->DataTypeConversion12 -

15852
      _rtB->DataTypeConversion20, _rtB->DataTypeConversion9 -

15853
      _rtB->DataTypeConversion21) * 180.0F / 3.14159274F -

15854
      AFCS_MODEL1_rt_atan2f_snf(_rtB->veast, _rtB->vnorth) * 180.0F /

15855
      3.14159274F;

15856
    _rtB->PsdNorthRef = PsdNorthRef;

15857
    _rtB->DireFg = (real32_T)DireFg;

15858


15859
    /* End of MATLAB Function: '<S293>/CompTP' */

15860


15861
    /* Switch: '<S293>/Switch' */

15862
    if (_rtB->UnitDelay3 > 0) {

15863
      _rtB->Switch_oz[0] = _rtB->HrefAL;

15864
      _rtB->Switch_oz[1] = _rtB->Xrw;

15865
      _rtB->Switch_oz[2] = _rtB->Yrw;

15866
      _rtB->Switch_oz[3] = _rtB->PsdNorthRef;

15867
      _rtB->Switch_oz[4] = _rtB->Xrw;

15868
      _rtB->Switch_oz[5] = _rtB->vxrw_a;

15869
      _rtB->Switch_oz[6] = _rtB->vyrw_n;

15870
    } else {

15871
      _rtB->Switch_oz[0] = _rtB->Href;

15872
      _rtB->Switch_oz[1] = _rtB->dXac;

15873
      _rtB->Switch_oz[2] = _rtB->dYac;

15874
      _rtB->Switch_oz[3] = _rtB->PSDref_n;

15875
      _rtB->Switch_oz[4] = _rtB->dXac;

15876
      _rtB->Switch_oz[5] = _rtB->vxrw_j;

15877
      _rtB->Switch_oz[6] = _rtB->vyrw_f;

15878
    }

15879


15880
    /* End of Switch: '<S293>/Switch' */

15881


15882
    /* DataTypeConversion: '<S293>/Data Type Conversion6' */

15883
    _rtB->vyrw = _rtB->Switch_oz[6];

15884


15885
    /* DataTypeConversion: '<S293>/Data Type Conversion5' */

15886
    _rtB->vxrw = _rtB->Switch_oz[5];

15887


15888
    /* Gain: '<S24>/Gain5' */

15889
    _rtB->Gain5_j = -_rtB->vxrw;

15890
  }

15891


15892
  if (ssIsSampleHit(S, 1, 0)) {

15893
    /* Chart: '<S24>/zeros2' */

15894
    /* Gateway: AFCS_MODEL1/GNC/NAV/zeros2 */

15895
    /* During: AFCS_MODEL1/GNC/NAV/zeros2 */

15896
    if (((uint8_T *)ssGetDWork(S, 586))[0] == 0U) {

15897
      /* Entry: AFCS_MODEL1/GNC/NAV/zeros2 */

15898
      ((uint8_T *)ssGetDWork(S, 586))[0] = 1U;

15899


15900
      /* Entry Internal: AFCS_MODEL1/GNC/NAV/zeros2 */

15901
      ((uint8_T *)ssGetDWork(S, 587))[0] = AFCS_MODEL1_IN_zero;

15902


15903
      /* Entry 'zero': '<S294>:38' */

15904
      _rtB->output = _rtB->Gain5_j + 1.0E-5F;

15905
    } else {

15906
      /* During 'zero': '<S294>:38' */

15907
      /* Transition: '<S294>:151' */

15908
      if ((_rtB->Gain5_j >= 0.0F) && (_rtB->Gain5_j < 1.0E-5F)) {

15909
        /* Transition: '<S294>:149' */

15910
        _rtB->output = 1.0E-5F;

15911
      } else if ((_rtB->Gain5_j < 0.0F) && (_rtB->Gain5_j > -1.0E-5F)) {

15912
        /* Transition: '<S294>:153' */

15913
        _rtB->output = -1.0E-5F;

15914
      } else {

15915
        /* Transition: '<S294>:155' */

15916
        _rtB->output = _rtB->Gain5_j;

15917
      }

15918
    }

15919


15920
    /* End of Chart: '<S24>/zeros2' */

15921


15922
    /* Saturate: '<S7>/Saturation1' incorporates:

15923
     *  Constant: '<S7>/Constant3'

15924
     */

15925
    _rtB->Saturation1_g = 0.005F;

15926
  }

15927


15928
  if (1) {

15929
    /* Switch: '<S24>/Switch' incorporates:

15930
     *  Constant: '<S24>/Constant4'

15931
     */

15932
    if (_rtB->LogicalOperator1_b) {

15933
      _rtB->UnaryMinus = 0.0;

15934
    } else {

15935
      /* Trigonometry: '<S24>/Trigonometric Function2' */

15936
      _rtB->TrigonometricFunction2 = AFCS_MODEL1_rt_atan2f_snf(_rtB->vyrw,

15937
        _rtB->output);

15938


15939
      /* Gain: '<S283>/Unit Conversion' */

15940
      _rtB->UnitConversion_a = 57.2957802F * _rtB->TrigonometricFunction2;

15941
      _rtB->UnaryMinus = _rtB->UnitConversion_a;

15942
    }

15943


15944
    /* End of Switch: '<S24>/Switch' */

15945


15946
    /* DataTypeConversion: '<S24>/Data Type Conversion8' */

15947
    _rtB->psd = (real32_T)_rtB->UnaryMinus;

15948


15949
    /* DataTypeConversion: '<S293>/Data Type Conversion1' */

15950
    _rtB->yrw = _rtB->Switch_oz[2];

15951


15952
    /* DataTypeConversion: '<S13>/Data Type Conversion32' */

15953
    _rtB->UnaryMinus = _rtB->Ndefine3;

15954


15955
    /* Gain: '<S13>/Gain21' */

15956
    _rtB->UnaryMinus *= 0.001;

15957


15958
    /* DataTypeConversion: '<S24>/Data Type Conversion2' */

15959
    _rtB->Gain_e = (real32_T)_rtB->UnaryMinus;

15960


15961
    /* Gain: '<S24>/Gain6' */

15962
    _rtB->Qbar_d = 0.101936802F * _rtB->Gain_e;

15963


15964
    /* DataTypeConversion: '<S24>/Data Type Conversion6' */

15965
    _rtB->DataTypeConversion6 = _rtB->altitude;

15966


15967
    /* MATLAB Function: '<S24>/Embedded MATLAB Function1' */

15968
    /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/Embedded MATLAB Function1': '<S292>:1' */

15969
    /* %%%%%%% */

15970
    /* '<S292>:1:3' */

15971
    /* '<S292>:1:4' */

15972
    TrigonometricFunction4_o = 6.371004E+6 * _rtB->DataTypeConversion6 /

15973
      (6.371004E+6 + _rtB->DataTypeConversion6);

15974


15975
    /* %%%%%% */

15976
    if (_rtB->DataTypeConversion6 <= 11019.0) {

15977
      /* '<S292>:1:6' */

15978
      /* '<S292>:1:7' */

15979
      _rtB->rou_f = AFCS_MODEL1_rt_powd_snf(1.0 - 2.2557700000000003E-5 *

15980
        TrigonometricFunction4_o, 4.25588) * 1.225;

15981
    } else if (_rtB->DataTypeConversion6 <= 20063.0) {

15982
      /* '<S292>:1:8' */

15983
      /* '<S292>:1:9' */

15984
      _rtB->rou_f = exp((TrigonometricFunction4_o - 11000.0) *

15985
                        -0.00015768850000000002) * 0.3639176;

15986
    } else if (_rtB->DataTypeConversion6 <= 32162.0) {

15987
      /* '<S292>:1:10' */

15988
      /* '<S292>:1:11' */

15989
      _rtB->rou_f = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 20000.0)

15990
        * 4.6157399999999991E-6 + 1.0, -35.16322) * 0.08803471;

15991
    } else if (_rtB->DataTypeConversion6 <= 47350.0) {

15992
      /* '<S292>:1:12' */

15993
      /* '<S292>:1:13' */

15994
      _rtB->rou_f = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 32000.0)

15995
        * 1.224579E-5 + 1.0, -13.20115) * 0.01322497;

15996
    } else if (_rtB->DataTypeConversion6 <= 51413.0) {

15997
      /* '<S292>:1:14' */

15998
      /* '<S292>:1:15' */

15999
      _rtB->rou_f = exp((TrigonometricFunction4_o - 47000.0) *

16000
                        -0.00012622660000000002) * 0.001427527;

16001
    } else if (_rtB->DataTypeConversion6 < 71802.0) {

16002
      /* '<S292>:1:16' */

16003
      /* '<S292>:1:17' */

16004
      _rtB->rou_f = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -

16005
        51000.0) * 1.0345459999999998E-5, 11.20115) * 0.00086160110000000007;

16006
    } else if (_rtB->DataTypeConversion6 < 86000.0) {

16007
      /* '<S292>:1:18' */

16008
      /* '<S292>:1:19' */

16009
      _rtB->rou_f = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -

16010
        71000.0) * 9.3174939999999987E-6, 16.08161) * 6.421057E-5;

16011
    } else {

16012
      /* '<S292>:1:21' */

16013
      _rtB->rou_f = exp((87284.8 - TrigonometricFunction4_o) / 5470.0) *

16014
        4.4603475E-6;

16015
    }

16016


16017
    /* End of MATLAB Function: '<S24>/Embedded MATLAB Function1' */

16018


16019
    /* Product: '<S24>/Divide1' */

16020
    _rtB->UnaryMinus = _rtB->rou_f / 1.2250000238418579;

16021


16022
    /* Sqrt: '<S24>/Sqrt1' */

16023
    _rtB->UnaryMinus = sqrt(_rtB->UnaryMinus);

16024


16025
    /* DataTypeConversion: '<S13>/Data Type Conversion29' */

16026
    _rtB->Switch_dn = _rtB->Va_k;

16027


16028
    /* Gain: '<S13>/Gain18' */

16029
    _rtB->Switch_dn *= 0.001;

16030


16031
    /* DataTypeConversion: '<S24>/Data Type Conversion1' */

16032
    _rtB->Va_l = (real32_T)_rtB->Switch_dn;

16033


16034
    /* Product: '<S24>/Product4' */

16035
    _rtB->Switch_dn = _rtB->UnaryMinus * _rtB->Va_l;

16036


16037
    /* DataTypeConversion: '<S24>/Data Type Conversion5' */

16038
    _rtB->Vc_p = (real32_T)_rtB->Switch_dn;

16039


16040
    /* Step: '<S1>/StartEngine1 ' */

16041
    if (ssGetTaskTime(S,0) < 0.5) {

16042
      _rtB->UnaryMinus = 0.0;

16043
    } else {

16044
      _rtB->UnaryMinus = 10.0;

16045
    }

16046


16047
    /* End of Step: '<S1>/StartEngine1 ' */

16048


16049
    /* DataTypeConversion: '<S10>/Data Type Conversion29' */

16050
    TrigonometricFunction4_o = floor(_rtB->UnaryMinus);

16051
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

16052
    {

16053
      TrigonometricFunction4_o = 0.0;

16054
    } else {

16055
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);

16056
    }

16057


16058
    _rtB->StartEngine_qgc = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)

16059
      (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)

16060
      (uint8_T)TrigonometricFunction4_o);

16061


16062
    /* End of DataTypeConversion: '<S10>/Data Type Conversion29' */

16063


16064
    /* DataTypeConversion: '<S10>/Data Type Conversion1' */

16065
    TrigonometricFunction4_o = floor(_rtB->u2u3_a);

16066
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

16067
    {

16068
      TrigonometricFunction4_o = 0.0;

16069
    } else {

16070
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);

16071
    }

16072


16073
    _rtB->TakeoffFg_qgc = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)

16074
      (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)

16075
      (uint8_T)TrigonometricFunction4_o);

16076


16077
    /* End of DataTypeConversion: '<S10>/Data Type Conversion1' */

16078


16079
    /* Step: '<S10>/Step1' */

16080
    _rtB->UnaryMinus = !(ssGetTaskTime(S,0) < 1.0);

16081


16082
    /* DataTypeConversion: '<S10>/Data Type Conversion4' */

16083
    TrigonometricFunction4_o = floor(_rtB->UnaryMinus);

16084
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

16085
    {

16086
      TrigonometricFunction4_o = 0.0;

16087
    } else {

16088
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);

16089
    }

16090


16091
    _rtB->INS_switch = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)

16092
      (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)

16093
      (uint8_T)TrigonometricFunction4_o);

16094


16095
    /* End of DataTypeConversion: '<S10>/Data Type Conversion4' */

16096


16097
    /* Step: '<S10>/Step2' */

16098
    _rtB->UnaryMinus = !(ssGetTaskTime(S,0) < 20.0);

16099


16100
    /* DataTypeConversion: '<S10>/Data Type Conversion5' */

16101
    TrigonometricFunction4_o = floor(_rtB->UnaryMinus);

16102
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

16103
    {

16104
      TrigonometricFunction4_o = 0.0;

16105
    } else {

16106
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);

16107
    }

16108


16109
    _rtB->INSIntegralClear_Fg = (int8_T)(TrigonometricFunction4_o < 0.0 ?

16110
      (int32_T)(int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)

16111
      (int8_T)(uint8_T)TrigonometricFunction4_o);

16112


16113
    /* End of DataTypeConversion: '<S10>/Data Type Conversion5' */

16114
  }

16115


16116
  if (ssIsSampleHit(S, 1, 0)) {

16117
    /* Memory: '<S9>/Memory1' */

16118
    _rtB->Memory1_px = ((boolean_T *)ssGetDWork(S, 612))[0];

16119


16120
    /* Memory: '<S9>/Memory2' */

16121
    _rtB->Memory2_o = ((int8_T *)ssGetDWork(S, 504))[0];

16122


16123
    /* UnitDelay: '<S9>/Unit Delay' */

16124
    _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 355))[0];

16125


16126
    /* UnitDelay: '<S1>/Unit Delay1' */

16127
    _rtB->UnitDelay1_l = ((int8_T *)ssGetDWork(S, 356))[0];

16128


16129
    /* Logic: '<S9>/Logical Operator' */

16130
    _rtB->LogicalOperator_j = (_rtB->UnitDelay5_b != 0) ^ (_rtB->UnitDelay1_l !=

16131
      0);

16132


16133
    /* Outputs for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */

16134
    /* UnitDelay: '<S423>/Output' */

16135
    _rtB->Output = ((uint64_T *)ssGetDWork(S, 1))[0];

16136


16137
    /* DataTypeConversion: '<S335>/Data Type Conversion' */

16138
    _rtB->DataTypeConversion_g = (uint32_T)_rtB->Output;

16139


16140
    /* End of Outputs for SubSystem: '<S9>/Trajectory planning,tracking' */

16141
  }

16142


16143
  /* Outputs for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */

16144
  if (1) {

16145
    /* SignalConversion: '<S424>/TmpSignal ConversionAt SFunction Inport1' incorporates:

16146
     *  MATLAB Function: '<S335>/UAV Trajectory Planning'

16147
     */

16148
    _rtB->TmpSignalConversionAtSFunctionI[0] = _rtB->Lat;

16149
    _rtB->TmpSignalConversionAtSFunctionI[1] = _rtB->Lon;

16150
    _rtB->TmpSignalConversionAtSFunctionI[2] = _rtB->Altitude;

16151
    _rtB->TmpSignalConversionAtSFunctionI[3] = _rtB->GPS_vnorth;

16152
    _rtB->TmpSignalConversionAtSFunctionI[4] = _rtB->GPS_veast;

16153
    _rtB->TmpSignalConversionAtSFunctionI[5] = _rtB->GPS_vdown;

16154


16155
    /* MATLAB Function: '<S335>/UAV Trajectory Planning' */

16156
    kk1 = _rtB->NumPoint;

16157


16158
    /* MATLAB Function 'AFCS_MODEL1/PlanningSystem_LLA/Trajectory planning,tracking/UAV Trajectory Planning': '<S424>:1' */

16159
    /* 轨迹规划模块：根据航点位置规划路径，路径由直线和圆弧组成 */

16160
    /* 输入： */

16161
    /* 1.UAVState,无人机当前位置、速度（pn,pe,pd,vn,ve） */

16162
    /* 2.WayPoints,航路点及速度矩阵，数据格式为（x1..xn;y1...yn;z1...zn;v1...vn） */

16163
    /* 3.Lengthofwp，航点个数 */

16164
    /* 4.Fimax0，最大滚转角 */

16165
    /* 5.flag_ChangePt，切点指令 */

16166
    /* 6.TRACK_ToPt_cmd，指令目标点 */

16167
    /* 7.Takeoff，起飞指令 */

16168
    /* 输出：轨迹参数 */

16169
    /* % */

16170
    /* %航点个数 */

16171
    /* 最多32个航点 */

16172
    if (_rtB->NumPoint > 32) {

16173
      /* '<S424>:1:16' */

16174
      /* '<S424>:1:17' */

16175
      kk1 = 32;

16176
    }

16177


16178
    /* % 航点个数 */

16179
    /* % */

16180
    /* 导航参数 */

16181
    /* % */

16182
    /* %当前目标航点 */

16183
    /* 指令目标点 */

16184
    /* 最小转弯半径 */

16185
    /* % */

16186
    /* *********************************************************************************% */

16187
    /* '<S424>:1:36' */

16188
    /* '<S424>:1:37' */

16189
    /* % */

16190
    /*  Timestamp = t; */

16191
    /* 北东下坐标系原点 */

16192
    /* '<S424>:1:45' */

16193
    Lat0 = _rtB->LLA0[0] * 0.017453292519943295;

16194


16195
    /* '<S424>:1:46' */

16196
    Lon0 = _rtB->LLA0[1] * 0.017453292519943295;

16197


16198
    /* '<S424>:1:47' */

16199
    /* '<S424>:1:48' */

16200
    /* '<S424>:1:49' */

16201
    /* '<S424>:1:50' */

16202
    /* '<S424>:1:51' */

16203
    AFCS_MODEL1_TransformLLATBDX(_rtB->TmpSignalConversionAtSFunctionI[1] *

16204
      0.017453292519943295, _rtB->TmpSignalConversionAtSFunctionI[0] *

16205
      0.017453292519943295, _rtB->TmpSignalConversionAtSFunctionI[2], Lon0, Lat0,

16206
      _rtB->LLA0[2], &pn, &pe, &pd);

16207


16208
    /* '<S424>:1:51' */

16209
    /* '<S424>:1:52' */

16210
    /*  pn = UAVState(1); */

16211
    /*  pe = UAVState(2); */

16212
    /*  pd = UAVState(3); */

16213
    /* '<S424>:1:57' */

16214
    /* '<S424>:1:58' */

16215
    /* '<S424>:1:59' */

16216
    psi = AFCS_MODEL1_rt_atan2d_snf(_rtB->TmpSignalConversionAtSFunctionI[4],

16217
      _rtB->TmpSignalConversionAtSFunctionI[3]);

16218


16219
    /* psi是速度的偏角，相对于北向，顺时针是正，0~2pi */

16220
    if (psi < 0.0) {

16221
      /* '<S424>:1:60' */

16222
      /* '<S424>:1:61' */

16223
      psi += 6.2831853071795862;

16224
    }

16225


16226
    /* '<S424>:1:64' */

16227
    /* 当前飞机的速度 */

16228
    /* % */

16229
    /* '<S424>:1:66' */

16230
    AFCS_MODEL1_linspace(waypiont_xnAll);

16231


16232
    /* linspace是均分计算指令，不知道为什么这么写，用zeros(1,IndataLengthMax)不好吗 */

16233
    /* '<S424>:1:67' */

16234
    AFCS_MODEL1_linspace(waypiont_yeAll);

16235


16236
    /* 航点的y坐标 */

16237
    /* '<S424>:1:68' */

16238
    AFCS_MODEL1_linspace(waypiont_zhAll);

16239


16240
    /* 航点的z坐标 */

16241
    /* '<S424>:1:69' */

16242
    waypiont_xnAll[0] = 0.0;

16243


16244
    /* 人为添加的第一个航点，是起点，位置是0 */

16245
    /* '<S424>:1:70' */

16246
    waypiont_yeAll[0] = 0.0;

16247


16248
    /* '<S424>:1:71' */

16249
    waypiont_zhAll[0] = 0.0;

16250


16251
    /* '<S424>:1:73' */

16252
    AFCS_MODEL1_linspace(waypiont_velAll);

16253


16254
    /* '<S424>:1:74' */

16255
    waypiont_velAll[0] = _rtB->A[3];

16256


16257
    /* 人为添加的第一个航点的速度，用航点文件里的第一个航点速度 */

16258
    /*  waypiont_velAll(1) = 20; */

16259
    /* 将以经纬高形式的航路点改为北东地 */

16260
    /* '<S424>:1:78' */

16261
    i_1 = kk1 + 1;

16262
    if (i_1 > 127) {

16263
      i_1 = 127;

16264
    }

16265


16266
    /* '<S424>:1:78' */

16267
    for (rtAction = 2; rtAction <= (int8_T)i_1; rtAction++) {

16268
      /* '<S424>:1:78' */

16269
      /* '<S424>:1:79' */

16270
      flag = rtAction - 1;

16271
      if (flag < -128) {

16272
        flag = -128;

16273
      }

16274


16275
      tmp_c = rtAction - 1;

16276
      if (tmp_c < -128) {

16277
        tmp_c = -128;

16278
      }

16279


16280
      tmp_d = rtAction - 1;

16281
      if (tmp_d < -128) {

16282
        tmp_d = -128;

16283
      }

16284


16285
      AFCS_MODEL1_TransformLLATBDX(_rtB->A[(flag - 1) << 2] *

16286
        0.017453292519943295, _rtB->A[1 + ((tmp_c - 1) << 2)] *

16287
        0.017453292519943295, _rtB->A[2 + ((tmp_d - 1) << 2)], Lon0, Lat0,

16288
        _rtB->LLA0[2], &b_0, &c_0, &unusedU0);

16289


16290
      /* '<S424>:1:79' */

16291
      waypiont_xnAll[rtAction - 1] = b_0;

16292


16293
      /* '<S424>:1:79' */

16294
      waypiont_yeAll[rtAction - 1] = c_0;

16295


16296
      /* '<S424>:1:80' */

16297
      flag = rtAction - 1;

16298
      if (flag < -128) {

16299
        flag = -128;

16300
      }

16301


16302
      waypiont_zhAll[rtAction - 1] = _rtB->A[((flag - 1) << 2) + 2];

16303


16304
      /* 航点的相对高度 */

16305
      /* '<S424>:1:81' */

16306
      flag = rtAction - 1;

16307
      if (flag < -128) {

16308
        flag = -128;

16309
      }

16310


16311
      waypiont_velAll[rtAction - 1] = _rtB->A[((flag - 1) << 2) + 3];

16312


16313
      /* 重新整理，实际航点比文件里的航点多了一个人为添加的起始原点 */

16314
      /*      waypiont_velAll(i) = 20; */

16315
    }

16316


16317
    /* % */

16318
    /* '<S424>:1:87' */

16319
    e_2 = 30.0;

16320
    AFCS_MODEL1_tand(&e_2);

16321
    B = 9.8 * e_2;

16322
    for (i_1 = 0; i_1 < 32; i_1++) {

16323
      RminAll[i_1] = waypiont_velAll[i_1] * waypiont_velAll[i_1] / B * 1.5;

16324
    }

16325


16326
    /* //最小转弯半径// */

16327
    /* % */

16328
    /* '<S424>:1:89' */

16329
    i_1 = kk1 + 1;

16330
    if (i_1 > 127) {

16331
      i_1 = 127;

16332
    }

16333


16334
    /*  增点了原点航点，航点数增加1 */

16335
    /* % */

16336
    /* 起飞 */

16337
    if (_rtB->LogicalOperator_j) {

16338
      /* '<S424>:1:94' */

16339
      /* 在起飞时/前，以飞机为航点0（消侧偏用，用于飞机不在航点0与航点1的连线上的情况） */

16340
      /* 测试用，实际飞行待测  2020年5月17日16:52:44 */

16341
      /* '<S424>:1:98' */

16342
      waypiont_xnAll[1] = pn;

16343


16344
      /* 把第二个航点设为飞机当前位置 */

16345
      /* '<S424>:1:99' */

16346
      waypiont_yeAll[1] = pe;

16347


16348
      /* 飞起模式下，进来这，不在起飞模式，yValues(2)的值就变了 */

16349
      /* '<S424>:1:100' */

16350
      waypiont_zhAll[1] = -pd;

16351


16352
      /* '<S424>:1:102' */

16353
      AFCS_MODEL1_PGT_CalculateRmin(waypiont_xnAll, waypiont_yeAll, (int8_T)i_1,

16354
        RminAll, RminAllNewCal);

16355


16356
      /* 对Rmin进行微调，得到调整后所有航点的最小转弯半径 */

16357
      /* '<S424>:1:103' */

16358
      AFCS_MODEL1_PGT_changewaypoints(waypiont_xnAll, waypiont_yeAll,

16359
        waypiont_zhAll, (int8_T)i_1, RminAllNewCal, waypiont_velAll, vx_qdout,

16360
        vy_qdout, xn_qdout, ye_qdout, zh_qdout);

16361


16362
      /* 切出点的速度和坐标 */

16363
      /* '<S424>:1:105' */

16364
      for (kk1 = 1; kk1 <= (int8_T)i_1; kk1++) {

16365
        /* '<S424>:1:105' */

16366
        /* '<S424>:1:106' */

16367
        ((real_T *)ssGetDWork(S, 12))[kk1 - 1] = RminAllNewCal[kk1 - 1];

16368


16369
        /* '<S424>:1:107' */

16370
        ((real_T *)ssGetDWork(S, 16))[kk1 - 1] = vx_qdout[kk1 - 1];

16371


16372
        /* 航点处的速度 */

16373
        /* '<S424>:1:108' */

16374
        ((real_T *)ssGetDWork(S, 17))[kk1 - 1] = vy_qdout[kk1 - 1];

16375


16376
        /* '<S424>:1:109' */

16377
        ((real_T *)ssGetDWork(S, 13))[kk1 - 1] = xn_qdout[kk1 - 1];

16378


16379
        /* 航点处的切点的坐标 */

16380
        /* '<S424>:1:110' */

16381
        ((real_T *)ssGetDWork(S, 14))[kk1 - 1] = ye_qdout[kk1 - 1];

16382


16383
        /* '<S424>:1:111' */

16384
        ((real_T *)ssGetDWork(S, 15))[kk1 - 1] = zh_qdout[kk1 - 1];

16385
      }

16386


16387
      /*  // 消除坐标原点对航线的影响  *****20190718 * ********** */

16388
      /* '<S424>:1:114' */

16389
      ((real_T *)ssGetDWork(S, 13))[1] = pn;

16390


16391
      /* 第二个航点的切点还是第二个航点位置 */

16392
      /* '<S424>:1:115' */

16393
      ((real_T *)ssGetDWork(S, 14))[1] = pe;

16394


16395
      /*  //在有数据变动时，将当前飞机位置设置为转弯点，即从当前点开始调整航线 */

16396
      /*  //飞机当前转弯点为TRACK_ToPt+1 */

16397
      /* 测试用，实际飞行待测  2020年5月17日16:52:44 */

16398
      /* '<S424>:1:119' */

16399
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16400
      if (qY > 65535U) {

16401
        qY = 65535U;

16402
      }

16403


16404
      ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1] = pn;

16405


16406
      /* '<S424>:1:120' */

16407
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16408
      if (qY > 65535U) {

16409
        qY = 65535U;

16410
      }

16411


16412
      ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1] = pe;

16413


16414
      /* '<S424>:1:121' */

16415
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16416
      if (qY > 65535U) {

16417
        qY = 65535U;

16418
      }

16419


16420
      ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1] = -pd;

16421


16422
      /*      vx(TRACK_ToPt+1) = vn; */

16423
      /*      vy(TRACK_ToPt+1) = ve; */

16424
    }

16425


16426
    /* % */

16427
    if (_rtB->Memory1_px) {

16428
      /* '<S424>:1:126' */

16429
      /* 看程序，在20s~20.02s时间段内，会变成1 */

16430
      /*  //在有数据变动时，将当前飞机位置设置为转弯点，即从当前点开始调整航线 */

16431
      /* //飞机当前转弯点为TRACK_ToPt+1 */

16432
      /* '<S424>:1:129' */

16433
      rtAction = _rtB->Memory2_o;

16434
      if (_rtB->Memory2_o < 0) {

16435
        rtAction = 0;

16436
      }

16437


16438
      ((uint16_T *)ssGetDWork(S, 351))[0] = (uint16_T)rtAction;

16439


16440
      /* '<S424>:1:130' */

16441
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16442
      if (qY > 65535U) {

16443
        qY = 65535U;

16444
      }

16445


16446
      ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1] = pn;

16447


16448
      /* '<S424>:1:131' */

16449
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16450
      if (qY > 65535U) {

16451
        qY = 65535U;

16452
      }

16453


16454
      ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1] = pe;

16455


16456
      /* '<S424>:1:132' */

16457
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16458
      if (qY > 65535U) {

16459
        qY = 65535U;

16460
      }

16461


16462
      ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1] = -pd;

16463


16464
      /* '<S424>:1:133' */

16465
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16466
      if (qY > 65535U) {

16467
        qY = 65535U;

16468
      }

16469


16470
      ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1] =

16471
        _rtB->TmpSignalConversionAtSFunctionI[3];

16472


16473
      /* '<S424>:1:134' */

16474
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16475
      if (qY > 65535U) {

16476
        qY = 65535U;

16477
      }

16478


16479
      ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1] =

16480
        _rtB->TmpSignalConversionAtSFunctionI[4];

16481


16482
      /*      if Vg > vValuesNew(TRACK_ToPt + 2) */

16483
      /*          RminNew(TRACK_ToPt + 2) = Vg * Vg / (9.8 * tand(Fimax)) * 1.3;%//以当前速度计算转弯半径 */

16484
      /*      else */

16485
      /*          RminNew(TRACK_ToPt + 2) = vValuesNew(TRACK_ToPt + 2)* vValuesNew(TRACK_ToPt + 2) / (9.8 * tand(Fimax)) * 1.3;%//以当前速度计算转弯半径 */

16486
      /*      end */

16487
    }

16488


16489
    /* % */

16490
    /* '<S424>:1:143' */

16491
    qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16492
    if (qY > 65535U) {

16493
      qY = 65535U;

16494
    }

16495


16496
    postoNextqd_idx_0 = pn - ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];

16497
    qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16498
    if (qY > 65535U) {

16499
      qY = 65535U;

16500
    }

16501


16502
    postoNextqd_idx_1 = pe - ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];

16503


16504
    /* 当前位置到下一航点的切点的向量 */

16505
    /* '<S424>:1:144' */

16506
    /*  相对位置点乘相对速度，是cos夹角 */

16507
    /* '<S424>:1:146' */

16508
    dclose = sqrt(postoNextqd_idx_0 * postoNextqd_idx_0 + postoNextqd_idx_1 *

16509
                  postoNextqd_idx_1);

16510


16511
    /* 飞机当前位置距离下一航点的切点的距离 */

16512
    qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16513
    if (qY > 65535U) {

16514
      qY = 65535U;

16515
    }

16516


16517
    tmp_a = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16518
    if (tmp_a > 65535U) {

16519
      tmp_a = 65535U;

16520
    }

16521


16522
    tmp_b = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16523
    if (tmp_b > 65535U) {

16524
      tmp_b = 65535U;

16525
    }

16526


16527
    if (((((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1] * postoNextqd_idx_0 +

16528
          ((real_T *)ssGetDWork(S, 17))[(int32_T)tmp_a - 1] * postoNextqd_idx_1 >

16529
          0.0) && (dclose < ((real_T *)ssGetDWork(S, 12))[(int32_T)tmp_b - 1])) ||

16530
        (dclose < sqrt(_rtB->TmpSignalConversionAtSFunctionI[3] *

16531
                       _rtB->TmpSignalConversionAtSFunctionI[3] +

16532
                       _rtB->TmpSignalConversionAtSFunctionI[4] *

16533
                       _rtB->TmpSignalConversionAtSFunctionI[4]) * 3.0)) {

16534
      /* '<S424>:1:148' */

16535
      /* 切航点的标志 */

16536
      /* 切点,切点后需重新计算 */

16537
      /* '<S424>:1:150' */

16538
      AFCS_MODEL1_PGT_CalculateRmin(waypiont_xnAll, waypiont_yeAll, (int8_T)i_1,

16539
        RminAll, RminAllNewCal);

16540


16541
      /* 为什么要重新计算，这些值都没变 */

16542
      /* '<S424>:1:151' */

16543
      AFCS_MODEL1_PGT_changewaypoints(waypiont_xnAll, waypiont_yeAll,

16544
        waypiont_zhAll, (int8_T)i_1, RminAllNewCal, waypiont_velAll, vx_qdout,

16545
        vy_qdout, xn_qdout, ye_qdout, zh_qdout);

16546


16547
      /* 航点的速度和航点的切点坐标 */

16548
      /* '<S424>:1:152' */

16549
      for (kk1 = 1; kk1 <= (int8_T)i_1; kk1++) {

16550
        /* '<S424>:1:152' */

16551
        /* '<S424>:1:153' */

16552
        ((real_T *)ssGetDWork(S, 12))[kk1 - 1] = RminAllNewCal[kk1 - 1];

16553


16554
        /* '<S424>:1:154' */

16555
        ((real_T *)ssGetDWork(S, 16))[kk1 - 1] = vx_qdout[kk1 - 1];

16556


16557
        /* '<S424>:1:155' */

16558
        ((real_T *)ssGetDWork(S, 17))[kk1 - 1] = vy_qdout[kk1 - 1];

16559


16560
        /* '<S424>:1:156' */

16561
        ((real_T *)ssGetDWork(S, 13))[kk1 - 1] = xn_qdout[kk1 - 1];

16562


16563
        /* '<S424>:1:157' */

16564
        ((real_T *)ssGetDWork(S, 14))[kk1 - 1] = ye_qdout[kk1 - 1];

16565


16566
        /* '<S424>:1:158' */

16567
        ((real_T *)ssGetDWork(S, 15))[kk1 - 1] = zh_qdout[kk1 - 1];

16568
      }

16569


16570
      /* '<S424>:1:160' */

16571
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16572
      if (qY > 65535U) {

16573
        qY = 65535U;

16574
      }

16575


16576
      ((uint16_T *)ssGetDWork(S, 351))[0] = (uint16_T)qY;

16577


16578
      /* 当前航点更新 */

16579
      /*  // 消除坐标原点对航线的影响  *****20190718 * ********** */

16580
      /* '<S424>:1:162' */

16581
      ((real_T *)ssGetDWork(S, 13))[1] = waypiont_xnAll[1];

16582


16583
      /* '<S424>:1:163' */

16584
      ((real_T *)ssGetDWork(S, 14))[1] = waypiont_yeAll[1];

16585
    }

16586


16587
    /* % */

16588
    /* '<S424>:1:166' */

16589
    qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16590
    if (qY > 65535U) {

16591
      qY = 65535U;

16592
    }

16593


16594
    if ((int32_T)qY <= (int8_T)i_1) {

16595
      /* '<S424>:1:168' */

16596
      /* '<S424>:1:169' */

16597
      /* 当前转弯点 */

16598
      /* '<S424>:1:170' */

16599
      /* '<S424>:1:171' */

16600
      /* '<S424>:1:172' */

16601
      /* '<S424>:1:173' */

16602
      /* '<S424>:1:174' */

16603
      /* 下一转弯点 */

16604
      /* '<S424>:1:175' */

16605
      /* '<S424>:1:176' */

16606
      /* '<S424>:1:177' */

16607
      /* '<S424>:1:178' */

16608
      /* '<S424>:1:180' */

16609
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16610
      if (qY > 65535U) {

16611
        qY = 65535U;

16612
      }

16613


16614
      tmp_a = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16615
      if (tmp_a > 65535U) {

16616
        tmp_a = 65535U;

16617
      }

16618


16619
      tmp_b = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16620
      if (tmp_b > 65535U) {

16621
        tmp_b = 65535U;

16622
      }

16623


16624
      tmp_2 = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16625
      if (tmp_2 > 65535U) {

16626
        tmp_2 = 65535U;

16627
      }

16628


16629
      tmp_3 = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16630
      if (tmp_3 > 65535U) {

16631
        tmp_3 = 65535U;

16632
      }

16633


16634
      tmp_4 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16635
      if (tmp_4 > 65535U) {

16636
        tmp_4 = 65535U;

16637
      }

16638


16639
      tmp_5 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16640
      if (tmp_5 > 65535U) {

16641
        tmp_5 = 65535U;

16642
      }

16643


16644
      tmp_6 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16645
      if (tmp_6 > 65535U) {

16646
        tmp_6 = 65535U;

16647
      }

16648


16649
      tmp_7 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16650
      if (tmp_7 > 65535U) {

16651
        tmp_7 = 65535U;

16652
      }

16653


16654
      tmp_8 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16655
      if (tmp_8 > 65535U) {

16656
        tmp_8 = 65535U;

16657
      }

16658


16659
      tmp_9 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16660
      if (tmp_9 > 65535U) {

16661
        tmp_9 = 65535U;

16662
      }

16663


16664
      AFCS_MODE_PGT_Guidance3D_Dubins(((real_T *)ssGetDWork(S, 13))[(int32_T)qY

16665
        - 1], ((real_T *)ssGetDWork(S, 14))[(int32_T)tmp_a - 1], ((real_T *)

16666
        ssGetDWork(S, 15))[(int32_T)tmp_b - 1], ((real_T *)ssGetDWork(S, 16))

16667
        [(int32_T)tmp_2 - 1], ((real_T *)ssGetDWork(S, 17))[(int32_T)tmp_3 - 1],

16668
        ((real_T *)ssGetDWork(S, 13))[(int32_T)tmp_4 - 1], ((real_T *)ssGetDWork

16669
        (S, 14))[(int32_T)tmp_5 - 1], ((real_T *)ssGetDWork(S, 15))[(int32_T)

16670
        tmp_6 - 1], ((real_T *)ssGetDWork(S, 16))[(int32_T)tmp_7 - 1], ((real_T *)

16671
        ssGetDWork(S, 17))[(int32_T)tmp_8 - 1], ((real_T *)ssGetDWork(S, 12))

16672
        [(int32_T)tmp_9 - 1], &b_R, &unusedU1, &b_xq1, &b_yq1, &b_hq1, &b_fis,

16673
        &b_Lamda1, &b_xq2, &b_yq2, &b_hq2, &b_fi2, &b_Lamda2, &b_x0_dot,

16674
        &b_y0_dot, &b_tx_dot, &b_ty_dot, &b_fi1sum, &b_fi2sum);

16675


16676
      /* '<S424>:1:182' */

16677
      _rtB->TraParam[0] = b_R;

16678


16679
      /* '<S424>:1:183' */

16680
      _rtB->TraParam[1] = b_xq1;

16681


16682
      /* 圆1的切出点坐标x */

16683
      /* '<S424>:1:184' */

16684
      _rtB->TraParam[2] = b_yq1;

16685


16686
      /* 圆1的切出点坐标y */

16687
      /* '<S424>:1:185' */

16688
      _rtB->TraParam[3] = b_hq1;

16689


16690
      /* 圆1的切出点高度h */

16691
      /* '<S424>:1:186' */

16692
      _rtB->TraParam[4] = b_fis;

16693


16694
      /* 圆1的切出点角度 */

16695
      /* '<S424>:1:187' */

16696
      _rtB->TraParam[5] = b_Lamda1;

16697


16698
      /* 圆1是顺时针还是逆时针，-1是顺时针，1是逆时针 */

16699
      /* '<S424>:1:188' */

16700
      _rtB->TraParam[6] = b_xq2;

16701


16702
      /* 圆2的切入点坐标系x */

16703
      /* '<S424>:1:189' */

16704
      _rtB->TraParam[7] = b_yq2;

16705


16706
      /* 圆2的切入点坐标系y */

16707
      /* '<S424>:1:190' */

16708
      _rtB->TraParam[8] = b_hq2;

16709


16710
      /* 圆2的切入点高度h */

16711
      /* '<S424>:1:191' */

16712
      _rtB->TraParam[9] = b_fi2;

16713


16714
      /* 圆2上的切入点角度 */

16715
      /* '<S424>:1:192' */

16716
      _rtB->TraParam[10] = b_Lamda2;

16717


16718
      /* 圆2是顺时针还是逆时针 */

16719
      /* '<S424>:1:193' */

16720
      _rtB->TraParam[11] = b_x0_dot;

16721


16722
      /* 圆1的圆心坐标x */

16723
      /* '<S424>:1:194' */

16724
      _rtB->TraParam[12] = b_y0_dot;

16725


16726
      /* 圆1的圆心坐标y */

16727
      /* '<S424>:1:195' */

16728
      _rtB->TraParam[13] = b_tx_dot;

16729


16730
      /* 圆2的圆心坐标x */

16731
      /* '<S424>:1:196' */

16732
      _rtB->TraParam[14] = b_ty_dot;

16733


16734
      /* 圆2的圆心坐标y */

16735
      /* '<S424>:1:197' */

16736
      _rtB->TraParam[15] = b_fi1sum;

16737


16738
      /* '<S424>:1:198' */

16739
      _rtB->TraParam[16] = b_fi2sum;

16740


16741
      /* '<S424>:1:199' */

16742
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16743
      if (qY > 65535U) {

16744
        qY = 65535U;

16745
      }

16746


16747
      _rtB->TraParam[17] = ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];

16748


16749
      /* 当前转弯点坐标x */

16750
      /* '<S424>:1:200' */

16751
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16752
      if (qY > 65535U) {

16753
        qY = 65535U;

16754
      }

16755


16756
      _rtB->TraParam[18] = ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];

16757


16758
      /* 当前转弯点坐标y */

16759
      /* '<S424>:1:201' */

16760
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16761
      if (qY > 65535U) {

16762
        qY = 65535U;

16763
      }

16764


16765
      _rtB->TraParam[19] = ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1];

16766


16767
      /* 当前转弯点的高度 */

16768
      /* '<S424>:1:202' */

16769
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16770
      if (qY > 65535U) {

16771
        qY = 65535U;

16772
      }

16773


16774
      _rtB->TraParam[20] = ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1];

16775


16776
      /* 当前转弯点速度vx */

16777
      /* '<S424>:1:203' */

16778
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16779
      if (qY > 65535U) {

16780
        qY = 65535U;

16781
      }

16782


16783
      _rtB->TraParam[21] = ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1];

16784


16785
      /* 当前转弯点速度vy */

16786
      /* '<S424>:1:204' */

16787
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16788
      if (qY > 65535U) {

16789
        qY = 65535U;

16790
      }

16791


16792
      _rtB->TraParam[22] = ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];

16793


16794
      /* 下一转弯点坐标x */

16795
      /* '<S424>:1:205' */

16796
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16797
      if (qY > 65535U) {

16798
        qY = 65535U;

16799
      }

16800


16801
      _rtB->TraParam[23] = ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];

16802


16803
      /* 下一转弯点坐标y */

16804
      /* '<S424>:1:206' */

16805
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16806
      if (qY > 65535U) {

16807
        qY = 65535U;

16808
      }

16809


16810
      _rtB->TraParam[24] = ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1];

16811


16812
      /* 下一转弯高度 */

16813
      /* '<S424>:1:207' */

16814
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16815
      if (qY > 65535U) {

16816
        qY = 65535U;

16817
      }

16818


16819
      _rtB->TraParam[25] = ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1];

16820


16821
      /* 下一转弯点速度vx */

16822
      /* '<S424>:1:208' */

16823
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16824
      if (qY > 65535U) {

16825
        qY = 65535U;

16826
      }

16827


16828
      _rtB->TraParam[26] = ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1];

16829


16830
      /* 下一转弯点速度vy */

16831
      /* '<S424>:1:209' */

16832
      _rtB->TraParam[27] = pn;

16833


16834
      /* 当前飞机位置x */

16835
      /* '<S424>:1:210' */

16836
      _rtB->TraParam[28] = pe;

16837


16838
      /* 当前飞机位置y */

16839
      /* '<S424>:1:211' */

16840
      _rtB->TraParam[29] = -pd;

16841


16842
      /* 当前飞机高度 */

16843
      /* '<S424>:1:212' */

16844
      _rtB->TraParam[30] = _rtB->TmpSignalConversionAtSFunctionI[3];

16845


16846
      /* 当前飞机速度vx */

16847
      /* '<S424>:1:213' */

16848
      _rtB->TraParam[31] = _rtB->TmpSignalConversionAtSFunctionI[4];

16849


16850
      /* 当前飞机速度vy */

16851
      /* '<S424>:1:214' */

16852
      _rtB->TraParam[32] = psi;

16853


16854
      /* 当前飞机的速度偏角（相对于北向） */

16855
      /* '<S424>:1:215' */

16856
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16857
      if (qY > 65535U) {

16858
        qY = 65535U;

16859
      }

16860


16861
      _rtB->TraParam[33] = waypiont_velAll[(int32_T)qY - 1];

16862


16863
      /* 下一航点的速度 */

16864
      qY = _rtB->DataTypeConversion_g - ((uint32_T *)ssGetDWork(S, 343))[0];

16865
      if (qY > _rtB->DataTypeConversion_g) {

16866
        qY = 0U;

16867
      }

16868


16869
      if (qY > 100U) {

16870
        /* '<S424>:1:239' */

16871
        /* 飞机的当前位置 */

16872
        /* '<S424>:1:241' */

16873
        ((uint32_T *)ssGetDWork(S, 343))[0] = _rtB->DataTypeConversion_g;

16874
      }

16875
    } else {

16876
      /* '<S424>:1:246' */

16877
      ((uint16_T *)ssGetDWork(S, 351))[0] = 0U;

16878


16879
      /* '<S424>:1:247' */

16880
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16881
      if (qY > 65535U) {

16882
        qY = 65535U;

16883
      }

16884


16885
      ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1] = pn;

16886


16887
      /* '<S424>:1:248' */

16888
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16889
      if (qY > 65535U) {

16890
        qY = 65535U;

16891
      }

16892


16893
      ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1] = pe;

16894


16895
      /* '<S424>:1:249' */

16896
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16897
      if (qY > 65535U) {

16898
        qY = 65535U;

16899
      }

16900


16901
      ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1] = -pd;

16902


16903
      /* '<S424>:1:250' */

16904
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16905
      if (qY > 65535U) {

16906
        qY = 65535U;

16907
      }

16908


16909
      ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1] =

16910
        _rtB->TmpSignalConversionAtSFunctionI[3];

16911


16912
      /* '<S424>:1:251' */

16913
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16914
      if (qY > 65535U) {

16915
        qY = 65535U;

16916
      }

16917


16918
      ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1] =

16919
        _rtB->TmpSignalConversionAtSFunctionI[4];

16920


16921
      /* '<S424>:1:253' */

16922
      /* '<S424>:1:254' */

16923
      /* '<S424>:1:255' */

16924
      /* '<S424>:1:256' */

16925
      /* '<S424>:1:257' */

16926
      /* '<S424>:1:258' */

16927
      /* '<S424>:1:259' */

16928
      /* '<S424>:1:260' */

16929
      /* '<S424>:1:261' */

16930
      /* '<S424>:1:262' */

16931
      /* '<S424>:1:264' */

16932
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16933
      if (qY > 65535U) {

16934
        qY = 65535U;

16935
      }

16936


16937
      tmp_a = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16938
      if (tmp_a > 65535U) {

16939
        tmp_a = 65535U;

16940
      }

16941


16942
      tmp_b = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16943
      if (tmp_b > 65535U) {

16944
        tmp_b = 65535U;

16945
      }

16946


16947
      tmp_2 = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16948
      if (tmp_2 > 65535U) {

16949
        tmp_2 = 65535U;

16950
      }

16951


16952
      tmp_3 = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

16953
      if (tmp_3 > 65535U) {

16954
        tmp_3 = 65535U;

16955
      }

16956


16957
      tmp_4 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16958
      if (tmp_4 > 65535U) {

16959
        tmp_4 = 65535U;

16960
      }

16961


16962
      tmp_5 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16963
      if (tmp_5 > 65535U) {

16964
        tmp_5 = 65535U;

16965
      }

16966


16967
      tmp_6 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16968
      if (tmp_6 > 65535U) {

16969
        tmp_6 = 65535U;

16970
      }

16971


16972
      tmp_7 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16973
      if (tmp_7 > 65535U) {

16974
        tmp_7 = 65535U;

16975
      }

16976


16977
      tmp_8 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16978
      if (tmp_8 > 65535U) {

16979
        tmp_8 = 65535U;

16980
      }

16981


16982
      tmp_9 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

16983
      if (tmp_9 > 65535U) {

16984
        tmp_9 = 65535U;

16985
      }

16986


16987
      AFCS_MODE_PGT_Guidance3D_Dubins(((real_T *)ssGetDWork(S, 13))[(int32_T)qY

16988
        - 1], ((real_T *)ssGetDWork(S, 14))[(int32_T)tmp_a - 1], ((real_T *)

16989
        ssGetDWork(S, 15))[(int32_T)tmp_b - 1], ((real_T *)ssGetDWork(S, 16))

16990
        [(int32_T)tmp_2 - 1], ((real_T *)ssGetDWork(S, 17))[(int32_T)tmp_3 - 1],

16991
        ((real_T *)ssGetDWork(S, 13))[(int32_T)tmp_4 - 1], ((real_T *)ssGetDWork

16992
        (S, 14))[(int32_T)tmp_5 - 1], ((real_T *)ssGetDWork(S, 15))[(int32_T)

16993
        tmp_6 - 1], ((real_T *)ssGetDWork(S, 16))[(int32_T)tmp_7 - 1], ((real_T *)

16994
        ssGetDWork(S, 17))[(int32_T)tmp_8 - 1], ((real_T *)ssGetDWork(S, 12))

16995
        [(int32_T)tmp_9 - 1], &R, &unusedU2, &xq1, &yq1, &hq1, &fis, &Lamda1,

16996
        &xq2, &yq2, &hq2, &fi2, &Lamda2, &x0_dot, &y0_dot, &tx_dot, &ty_dot,

16997
        &fi1sum, &fi2sum);

16998


16999
      /* '<S424>:1:266' */

17000
      _rtB->TraParam[0] = R;

17001


17002
      /* '<S424>:1:267' */

17003
      _rtB->TraParam[1] = xq1;

17004


17005
      /* '<S424>:1:268' */

17006
      _rtB->TraParam[2] = yq1;

17007


17008
      /* '<S424>:1:269' */

17009
      _rtB->TraParam[3] = hq1;

17010


17011
      /* '<S424>:1:270' */

17012
      _rtB->TraParam[4] = fis;

17013


17014
      /* '<S424>:1:271' */

17015
      _rtB->TraParam[5] = Lamda1;

17016


17017
      /* '<S424>:1:272' */

17018
      _rtB->TraParam[6] = xq2;

17019


17020
      /* '<S424>:1:273' */

17021
      _rtB->TraParam[7] = yq2;

17022


17023
      /* '<S424>:1:274' */

17024
      _rtB->TraParam[8] = hq2;

17025


17026
      /* '<S424>:1:275' */

17027
      _rtB->TraParam[9] = fi2;

17028


17029
      /* '<S424>:1:276' */

17030
      _rtB->TraParam[10] = Lamda2;

17031


17032
      /* '<S424>:1:277' */

17033
      _rtB->TraParam[11] = x0_dot;

17034


17035
      /* '<S424>:1:278' */

17036
      _rtB->TraParam[12] = y0_dot;

17037


17038
      /* '<S424>:1:279' */

17039
      _rtB->TraParam[13] = tx_dot;

17040


17041
      /* '<S424>:1:280' */

17042
      _rtB->TraParam[14] = ty_dot;

17043


17044
      /* '<S424>:1:281' */

17045
      _rtB->TraParam[15] = fi1sum;

17046


17047
      /* '<S424>:1:282' */

17048
      _rtB->TraParam[16] = fi2sum;

17049


17050
      /* '<S424>:1:283' */

17051
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

17052
      if (qY > 65535U) {

17053
        qY = 65535U;

17054
      }

17055


17056
      _rtB->TraParam[17] = ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];

17057


17058
      /* '<S424>:1:284' */

17059
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

17060
      if (qY > 65535U) {

17061
        qY = 65535U;

17062
      }

17063


17064
      _rtB->TraParam[18] = ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];

17065


17066
      /* '<S424>:1:285' */

17067
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

17068
      if (qY > 65535U) {

17069
        qY = 65535U;

17070
      }

17071


17072
      _rtB->TraParam[19] = ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1];

17073


17074
      /* '<S424>:1:286' */

17075
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

17076
      if (qY > 65535U) {

17077
        qY = 65535U;

17078
      }

17079


17080
      _rtB->TraParam[20] = ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1];

17081


17082
      /* '<S424>:1:287' */

17083
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;

17084
      if (qY > 65535U) {

17085
        qY = 65535U;

17086
      }

17087


17088
      _rtB->TraParam[21] = ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1];

17089


17090
      /* '<S424>:1:288' */

17091
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

17092
      if (qY > 65535U) {

17093
        qY = 65535U;

17094
      }

17095


17096
      _rtB->TraParam[22] = ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];

17097


17098
      /* '<S424>:1:289' */

17099
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

17100
      if (qY > 65535U) {

17101
        qY = 65535U;

17102
      }

17103


17104
      _rtB->TraParam[23] = ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];

17105


17106
      /* '<S424>:1:290' */

17107
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

17108
      if (qY > 65535U) {

17109
        qY = 65535U;

17110
      }

17111


17112
      _rtB->TraParam[24] = ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1];

17113


17114
      /* '<S424>:1:291' */

17115
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

17116
      if (qY > 65535U) {

17117
        qY = 65535U;

17118
      }

17119


17120
      _rtB->TraParam[25] = ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1];

17121


17122
      /* '<S424>:1:292' */

17123
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

17124
      if (qY > 65535U) {

17125
        qY = 65535U;

17126
      }

17127


17128
      _rtB->TraParam[26] = ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1];

17129


17130
      /* '<S424>:1:293' */

17131
      _rtB->TraParam[27] = pn;

17132


17133
      /* '<S424>:1:294' */

17134
      _rtB->TraParam[28] = pe;

17135


17136
      /* '<S424>:1:295' */

17137
      _rtB->TraParam[29] = -pd;

17138


17139
      /* '<S424>:1:296' */

17140
      _rtB->TraParam[30] = _rtB->TmpSignalConversionAtSFunctionI[3];

17141


17142
      /* '<S424>:1:297' */

17143
      _rtB->TraParam[31] = _rtB->TmpSignalConversionAtSFunctionI[4];

17144


17145
      /* '<S424>:1:298' */

17146
      _rtB->TraParam[32] = psi;

17147


17148
      /* '<S424>:1:299' */

17149
      qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;

17150
      if (qY > 65535U) {

17151
        qY = 65535U;

17152
      }

17153


17154
      _rtB->TraParam[33] = waypiont_velAll[(int32_T)qY - 1];

17155
    }

17156


17157
    /* '<S424>:1:302' */

17158
    _rtB->currentPoint = ((uint16_T *)ssGetDWork(S, 351))[0];

17159
    _rtB->pn = pn;

17160
    _rtB->pe = pe;

17161
    _rtB->ph = -pd;

17162
  }

17163


17164
  if (ssIsSampleHit(S, 1, 0)) {

17165
    /* SignalConversion: '<S335>/TmpSignal ConversionAtTo WorkspaceInport1' */

17166
    _rtB->TmpSignalConversionAtToWorksp_o[0] = _rtB->pn;

17167
    _rtB->TmpSignalConversionAtToWorksp_o[1] = _rtB->pe;

17168
    _rtB->TmpSignalConversionAtToWorksp_o[2] = _rtB->ph;

17169


17170
    /* Sum: '<S426>/FixPt Sum1' incorporates:

17171
     *  Constant: '<S426>/FixPt Constant'

17172
     */

17173
    _rtB->FixPtSum1 = _rtB->Output + 1ULL;

17174


17175
    /* Switch: '<S427>/FixPt Switch' incorporates:

17176
     *  Constant: '<S427>/Constant'

17177
     */

17178
    if (_rtB->FixPtSum1 > 4294967296ULL) {

17179
      _rtB->FixPtSwitch = 0ULL;

17180
    } else {

17181
      _rtB->FixPtSwitch = _rtB->FixPtSum1;

17182
    }

17183


17184
    /* End of Switch: '<S427>/FixPt Switch' */

17185
  }

17186


17187
  if (1) {

17188
    /* MATLAB Function: '<S335>/UAV Trajectory Tracking' incorporates:

17189
     *  Constant: '<S9>/nav_param'

17190
     */

17191
    /* MATLAB Function 'AFCS_MODEL1/PlanningSystem_LLA/Trajectory planning,tracking/UAV Trajectory Tracking': '<S425>:1' */

17192
    /* 轨迹跟踪模块：跟踪直线和圆弧轨迹 */

17193
    /* 轨迹由圆弧、直线、圆弧组成，轨迹参数定义在输入参数里 */

17194
    /* 输入： */

17195
    /* 1.TraParam，跟踪轨迹参数 */

17196
    /* 2.nav_param，导航参数 */

17197
    /* 输出： */

17198
    /* 1.hc，高度指令 */

17199
    /* 2.psic，航向角指令 */

17200
    /* 3.epy，侧偏距 */

17201
    /* 4.Vc，速度大小指令 */

17202
    /* 5.acmd，侧向加速度 */

17203
    /*  */

17204
    /* '<S425>:1:16' */

17205
    /* 半径 */

17206
    /* '<S425>:1:17' */

17207
    /* 圆1的切出点坐标x,有时，圆1的切出点和圆1的切点相同，有时有一些差别 */

17208
    /* '<S425>:1:18' */

17209
    /* 圆1的切出点坐标y */

17210
    /* 圆1的切出点高度h */

17211
    /* 圆1的切出点角度，与北方向的夹角 */

17212
    /* '<S425>:1:21' */

17213
    /* 圆1是顺时针还是逆时针，-1是顺时针，1是逆时针 */

17214
    /* '<S425>:1:22' */

17215
    /* 圆2的切入点坐标系x */

17216
    /* '<S425>:1:23' */

17217
    /* 圆2的切入点坐标系y */

17218
    /* '<S425>:1:24' */

17219
    /* 圆2的切入点高度h */

17220
    /* 圆2上的切入点角度 */

17221
    /* '<S425>:1:26' */

17222
    /* 圆2是顺时针还是逆时针 */

17223
    /* '<S425>:1:27' */

17224
    /* 圆1的圆心坐标x */

17225
    /* '<S425>:1:28' */

17226
    /* 圆1的圆心坐标y */

17227
    /* '<S425>:1:29' */

17228
    /* 圆2的圆心坐标x */

17229
    /* '<S425>:1:30' */

17230
    /* 圆2的圆心坐标y */

17231
    /* '<S425>:1:31' */

17232
    /* '<S425>:1:32' */

17233
    /* '<S425>:1:33' */

17234
    /* 当前转弯点坐标x */

17235
    /* '<S425>:1:34' */

17236
    /* 当前转弯点坐标y */

17237
    /* '<S425>:1:35' */

17238
    /* 当前转弯点高度 */

17239
    /* '<S425>:1:36' */

17240
    /* 当前转弯点速度vx */

17241
    /* '<S425>:1:37' */

17242
    /* 当前转弯点速度vy */

17243
    /* '<S425>:1:38' */

17244
    /* 下一转弯点坐标x */

17245
    /* '<S425>:1:39' */

17246
    /* 下一转弯点坐标y */

17247
    /* '<S425>:1:40' */

17248
    /* 下一转弯点高度 */

17249
    /* '<S425>:1:41' */

17250
    /* 下一转弯点速度vx */

17251
    /* '<S425>:1:42' */

17252
    /* 下一转弯点速度vy */

17253
    /* '<S425>:1:43' */

17254
    /* 当前飞机位置x */

17255
    /* '<S425>:1:44' */

17256
    /* 当前飞机位置y */

17257
    /* '<S425>:1:45' */

17258
    /* 当前飞机高度 */

17259
    /* '<S425>:1:46' */

17260
    /* 当前飞机速度vx */

17261
    /* '<S425>:1:47' */

17262
    /* 当前飞机速度vy */

17263
    /* '<S425>:1:48' */

17264
    /* 当前飞机的速度偏角（相对于北向） */

17265
    /* '<S425>:1:49' */

17266
    _rtB->Vc = _rtB->TraParam[33];

17267


17268
    /* 下一航点的速度 */

17269
    /* 当前航点坐标x */

17270
    /* 当前航点坐标y */

17271
    /* 当前航点速度vx  */

17272
    /* 当前航点速度vy */

17273
    /* 圆1的切出点坐标x */

17274
    /* 圆1的切出点坐标y */

17275
    /* '<S425>:1:58' */

17276
    qsn = _rtB->TraParam[6] - _rtB->TraParam[1];

17277


17278
    /* '<S425>:1:59' */

17279
    qse = _rtB->TraParam[7] - _rtB->TraParam[2];

17280


17281
    /* 圆2的切入点坐标系x */

17282
    /* 圆2的切入点坐标系y */

17283
    /*  wen = pn;   %这是原来的程序，这块应该写错了 */

17284
    /*  wee = pe; */

17285
    /*  qen = vn; */

17286
    /*  qee = ve; */

17287
    /* 下一转弯点坐标x  */

17288
    /* 下一转弯点坐标y */

17289
    /* 下一转弯点速度vx */

17290
    /* 下一转弯点速度vy */

17291
    /* '<S425>:1:72' */

17292
    dR = 0.02 * _rtB->TraParam[0];

17293


17294
    /* '<S425>:1:74' */

17295
    _rtB->epy = 0.0;

17296


17297
    /* % */

17298
    /* 1 */

17299
    /*  gamac = Gama; */

17300
    /* '<S425>:1:83' */

17301
    /* '<S425>:1:91' */

17302
    qn = _rtB->TraParam[6] - _rtB->TraParam[1];

17303


17304
    /* 圆2的切入点坐标-圆1的切出点坐标 */

17305
    /* '<S425>:1:92' */

17306
    qe = _rtB->TraParam[7] - _rtB->TraParam[2];

17307


17308
    /* '<S425>:1:97' */

17309
    /*  gamac = Gama; */

17310
    /* '<S425>:1:100' */

17311
    /* % */

17312
    /* '<S425>:1:103' */

17313
    dps = sqrt((_rtB->TraParam[27] - _rtB->TraParam[1]) * (_rtB->TraParam[27] -

17314
                _rtB->TraParam[1]) + (_rtB->TraParam[28] - _rtB->TraParam[2]) *

17315
               (_rtB->TraParam[28] - _rtB->TraParam[2]));

17316


17317
    /* 飞机到圆1的切出点的距离 */

17318
    /* '<S425>:1:104' */

17319
    dpl = sqrt((_rtB->TraParam[27] - _rtB->TraParam[6]) * (_rtB->TraParam[27] -

17320
                _rtB->TraParam[6]) + (_rtB->TraParam[28] - _rtB->TraParam[7]) *

17321
               (_rtB->TraParam[28] - _rtB->TraParam[7]));

17322


17323
    /* 飞机到圆2的切入点的距离 */

17324
    /* % */

17325
    if ((((_rtB->TraParam[27] - _rtB->TraParam[1]) * qsn + (_rtB->TraParam[28] -

17326
           _rtB->TraParam[2]) * qse >= 0.0) && ((_rtB->TraParam[27] -

17327
           _rtB->TraParam[6]) * qsn + (_rtB->TraParam[28] - _rtB->TraParam[7]) *

17328
          qse <= 0.0)) || (dps < dR) || (dpl < dR)) {

17329
      /* '<S425>:1:106' */

17330
      /* 判断飞机跟踪直线的标志（参考说明文档） */

17331
      /* '<S425>:1:107' */

17332
      flag = 2;

17333


17334
      /* 跟踪直线 */

17335
    } else if (((_rtB->TraParam[27] - _rtB->TraParam[17]) * _rtB->TraParam[20] +

17336
                (_rtB->TraParam[28] - _rtB->TraParam[18]) * _rtB->TraParam[21] >

17337
                0.0) && ((_rtB->TraParam[27] - _rtB->TraParam[1]) * qsn +

17338
                         (_rtB->TraParam[28] - _rtB->TraParam[2]) * qse < 0.0))

17339
    {

17340
      /* '<S425>:1:108' */

17341
      /* 判断飞机跟踪第一段圆弧的标志 */

17342
      /* '<S425>:1:109' */

17343
      flag = 1;

17344
    } else if (((_rtB->TraParam[27] - _rtB->TraParam[6]) * qsn + (_rtB->

17345
                 TraParam[28] - _rtB->TraParam[7]) * qse > 0.0) &&

17346
               ((_rtB->TraParam[27] - _rtB->TraParam[22]) * _rtB->TraParam[25] +

17347
                (_rtB->TraParam[28] - _rtB->TraParam[23]) * _rtB->TraParam[26] <

17348
                0.0)) {

17349
      /* '<S425>:1:110' */

17350
      /* 判断飞机跟踪第二段圆弧的标志 */

17351
      /* '<S425>:1:111' */

17352
      flag = 3;

17353
    } else if ((_rtB->TraParam[27] - _rtB->TraParam[1]) * qsn + (_rtB->TraParam

17354
                [28] - _rtB->TraParam[2]) * qse < 0.0) {

17355
      /* '<S425>:1:112' */

17356
      /* '<S425>:1:113' */

17357
      flag = 1;

17358
    } else if ((_rtB->TraParam[27] - _rtB->TraParam[6]) * qsn + (_rtB->TraParam

17359
                [28] - _rtB->TraParam[7]) * qse > 0.0) {

17360
      /* '<S425>:1:114' */

17361
      /* '<S425>:1:115' */

17362
      flag = 3;

17363
    } else if ((_rtB->TraParam[27] - _rtB->TraParam[17]) * _rtB->TraParam[20] +

17364
               (_rtB->TraParam[28] - _rtB->TraParam[18]) * _rtB->TraParam[21] >

17365
               0.0) {

17366
      /* '<S425>:1:116' */

17367
      /* '<S425>:1:117' */

17368
      flag = 1;

17369
    } else {

17370
      /* '<S425>:1:119' */

17371
      flag = 3;

17372
    }

17373


17374
    if ((flag == 2) && (_rtB->TraParam[15] > 3.1415926535897931) &&

17375
        (_rtB->TraParam[15] < 4.71238898038469) && ((_rtB->TraParam[27] -

17376
          _rtB->TraParam[17]) * _rtB->TraParam[20] + (_rtB->TraParam[28] -

17377
          _rtB->TraParam[18]) * _rtB->TraParam[21] >= 0.0)) {

17378
      /* '<S425>:1:121' */

17379
      /* '<S425>:1:122' */

17380
      flag = 1;

17381
    } else {

17382
      if ((flag == 2) && (_rtB->TraParam[16] > 3.1415926535897931) &&

17383
          ((_rtB->TraParam[27] - _rtB->TraParam[22]) * _rtB->TraParam[25] +

17384
           (_rtB->TraParam[28] - _rtB->TraParam[23]) * _rtB->TraParam[26] < 0.0))

17385
      {

17386
        /* '<S425>:1:123' */

17387
        /* '<S425>:1:124' */

17388
        flag = 3;

17389
      }

17390
    }

17391


17392
    /* % */

17393
    /* %==============================平滑指令测试 2019年12月20日16:32:16===================================== */

17394
    /* '<S425>:1:129' */

17395
    Vg = sqrt(_rtB->TraParam[30] * _rtB->TraParam[30] + _rtB->TraParam[31] *

17396
              _rtB->TraParam[31]);

17397
    if ((flag == 1) && (dps < 4.0 * Vg)) {

17398
      /* '<S425>:1:130' */

17399
      /* 跟踪圆弧1时，飞机到圆弧1的切出点的距离太小，则直接跟踪直线 */

17400
      /* '<S425>:1:131' */

17401
      flag = 2;

17402
    }

17403


17404
    if ((flag == 2) && (dpl < 4.0 * Vg)) {

17405
      /* '<S425>:1:134' */

17406
      /* 跟踪直线时，飞机到圆弧2的切入点距离太小，则直接跟踪圆弧2 */

17407
      /* '<S425>:1:135' */

17408
      flag = 3;

17409
    }

17410


17411
    /* %==============================平滑指令测试 2019年12月20日16:32:16===================================== */

17412
    /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%2018/03/07%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

17413
    /* % */

17414
    if (flag == 1) {

17415
      /* '<S425>:1:141' */

17416
      /* '<S425>:1:142' */

17417
      PGT_followOrbit_nonlinear_guida(_rtB->TraParam[11], _rtB->TraParam[12],

17418
        _rtB->TraParam[0], -_rtB->TraParam[5], _rtB->TraParam[27],

17419
        _rtB->TraParam[28], _rtB->TraParam[30], _rtB->TraParam[31],

17420
        _rtB->TraParam[32], _rtB->TraParam[19], 4.0, &unusedU0_0, &b_fopsic,

17421
        &b_acmd);

17422


17423
      /* '<S425>:1:142' */

17424
      _rtB->acmd = b_acmd;

17425


17426
      /* 跟踪圆弧1 */

17427
      /* '<S425>:1:143' */

17428
      psic = b_fopsic;

17429
    } else if (flag == 3) {

17430
      /* '<S425>:1:144' */

17431
      /* '<S425>:1:145' */

17432
      PGT_followOrbit_nonlinear_guida(_rtB->TraParam[13], _rtB->TraParam[14],

17433
        _rtB->TraParam[0], -_rtB->TraParam[10], _rtB->TraParam[27],

17434
        _rtB->TraParam[28], _rtB->TraParam[30], _rtB->TraParam[31],

17435
        _rtB->TraParam[32], -_rtB->TraParam[8], 4.0, &unusedU1_0, &fopsic, &acmd);

17436


17437
      /* '<S425>:1:145' */

17438
      _rtB->acmd = acmd;

17439


17440
      /* 跟踪圆弧2 */

17441
      /* '<S425>:1:146' */

17442
      psic = fopsic;

17443
    } else {

17444
      /* '<S425>:1:148' */

17445
      /* followstraightline 此处显示有关此函数的摘要 */

17446
      /* Path definition r = (rn, re, rd)T , and q = (qn, qe, qd)T . MAV position p = (pn, pe, pd)T */

17447
      /* course psi, gains psi_infinte, kpath. */

17448
      /* 输入： */

17449
      /*  rn,re:圆1的切出点的坐标 */

17450
      /*  qn,qe:圆2的切入点相对于圆1的切出点的相对坐标 */

17451
      /*  pn,pe:飞机的当前位置 */

17452
      /*  vn,ve:飞机的速度 */

17453
      /*  psi:飞机的速度偏角（相对于北向） */

17454
      b_Vg = sqrt(_rtB->TraParam[30] * _rtB->TraParam[30] + _rtB->TraParam[31] *

17455
                  _rtB->TraParam[31]);

17456
      L1 = 4.0 * b_Vg;

17457


17458
      /*  L1 = 10*Vg; */

17459
      /* L1 = 50; */

17460
      Ru = sqrt((_rtB->TraParam[27] - _rtB->TraParam[1]) * (_rtB->TraParam[27] -

17461
                 _rtB->TraParam[1]) + (_rtB->TraParam[28] - _rtB->TraParam[2]) *

17462
                (_rtB->TraParam[28] - _rtB->TraParam[2]));

17463
      psiq = AFCS_MODEL1_rt_atan2d_snf(qe, qn);

17464


17465
      /*  if psiq < 0 */

17466
      /*      psiq = psiq + 2*pi; */

17467
      /*  end */

17468
      /*  if psiq-psi <= -pi */

17469
      /*      psiq = psiq + 2*pi; */

17470
      /*  end */

17471
      /*  if psiq-psi > pi */

17472
      /*      psiq = psiq - 2*pi; */

17473
      /*  end  */

17474
      epy = (_rtB->TraParam[27] - _rtB->TraParam[1]) * -sin(psiq) +

17475
        (_rtB->TraParam[28] - _rtB->TraParam[2]) * cos(psiq);

17476
      if (L1 * L1 - epy * epy > 0.0) {

17477
        delt = sqrt(L1 * L1 - epy * epy);

17478
      } else {

17479
        /*      delt = 50; */

17480
        delt = 200.0;

17481
      }

17482


17483
      if (Ru * Ru - epy * epy > 0.0) {

17484
        R_0 = sqrt(Ru * Ru - epy * epy);

17485
      } else {

17486
        R_0 = 100.0;

17487
      }

17488


17489
      /* ====================改进算法适用性 2019年12月20日17:32:44===================== */

17490
      if ((_rtB->TraParam[27] - _rtB->TraParam[1]) * qn + (_rtB->TraParam[28] -

17491
           _rtB->TraParam[2]) * qe > 0.0) {

17492
        xt = (delt + R_0) * cos(psiq) + _rtB->TraParam[1];

17493
        yt = (delt + R_0) * sin(psiq) + _rtB->TraParam[2];

17494
      } else {

17495
        xt = (delt - R_0) * cos(psiq) + _rtB->TraParam[1];

17496
        yt = (delt - R_0) * sin(psiq) + _rtB->TraParam[2];

17497
      }

17498


17499
      /* ====================改进算法适用性 2019年12月20日17:32:44===================== */

17500
      flpsic = AFCS_MODEL1_rt_atan2d_snf(yt - _rtB->TraParam[28], xt -

17501
        _rtB->TraParam[27]);

17502
      if (flpsic < 0.0) {

17503
        flpsic += 6.2831853071795862;

17504
      }

17505


17506
      eta = flpsic - _rtB->TraParam[32];

17507
      if (flpsic - _rtB->TraParam[32] < -3.1415926535897931) {

17508
        flpsic += 6.2831853071795862;

17509
      }

17510


17511
      if (flpsic - _rtB->TraParam[32] > 3.1415926535897931) {

17512
        flpsic -= 6.2831853071795862;

17513
      }

17514


17515
      /*  aaa = 1; */

17516
      /* 将相对关系画出来 */

17517
      /*  figure(1) */

17518
      /*  plot([re,re+qe],[rn,rn+qn],'LineStyle','--','Marker','o','LineWidth',2,'color','r');  %切入点和切出点的位置 */

17519
      /*  hold on */

17520
      /*  plot(pe,pn,'Marker','*','LineWidth',4);%飞机的位置 */

17521
      /*  plot(yt,xt,'Marker','o','color','b','LineWidth',4);%预瞄点的位置 */

17522
      /*  psi_deg = psi*57.3 */

17523
      /*  psic_deg = psic*57.3 */

17524
      /*  aa = 1; */

17525
      /*  psi_str = num2str(psi*57.3);psic_str = num2str(psic*57.3); */

17526
      /*  text(yt,xt,psic_str); */

17527
      /* '<S425>:1:148' */

17528
      _rtB->epy = epy;

17529


17530
      /* '<S425>:1:148' */

17531
      _rtB->acmd = 2.0 * b_Vg * b_Vg / sqrt((_rtB->TraParam[27] - xt) *

17532
        (_rtB->TraParam[27] - xt) + (_rtB->TraParam[28] - yt) * (_rtB->TraParam

17533
        [28] - yt)) * sin(eta);

17534


17535
      /* 跟踪直线，只有跟踪直线有侧偏距 */

17536
      /* '<S425>:1:149' */

17537
      psic = flpsic;

17538


17539
      /*      close all */

17540
    }

17541


17542
    /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%2018/03/07%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

17543
    /* % */

17544
    /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%2018/03/07%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

17545
    /*  psic = mod(psic, 2*pi); */

17546
    if (psic < 0.0) {

17547
      /* '<S425>:1:157' */

17548
      /* '<S425>:1:158' */

17549
      psic += 6.2831853071795862;

17550
    }

17551


17552
    /* '<S425>:1:160' */

17553
    _rtB->hc = _rtB->TraParam[24];

17554


17555
    /*  nav_param = 1.5;%导航参数，根据实际情况修改 */

17556
    /*  psic =0.0; */

17557
    /*  hc = 0.0; */

17558
    /*  epy = 0.0; */

17559
    /*  acmd = 0.0; */

17560
    /*  [ psic,hc,epy,acmd] = PGT_follow3Ddubins( R,xq1,yq1,hq1,fis,Lamda1,xq2,yq2,hq2,fi2,Lamda2,x0_dot,y0_dot,tx_dot,ty_dot,fi1sum,fi2sum, xxs, yys, zzs, vxs, vys, xxf, yyf, zzf,vxf, vyf, pn, pe, pd, vn,ve,psi,nav_param); */

17561
    _rtB->psic = psic;

17562


17563
    /* End of MATLAB Function: '<S335>/UAV Trajectory Tracking' */

17564


17565
    /* DataTypeConversion: '<S1>/Data Type Conversion3' */

17566
    _rtB->Gain_e = (real32_T)_rtB->hc;

17567


17568
    /* DataTypeConversion: '<S10>/Data Type Conversion14' */

17569
    _rtB->ascmd_n = (real32_T)_rtB->acmd;

17570


17571
    /* DataTypeConversion: '<S9>/Data Type Conversion38' */

17572
    _rtB->StartEngine_qgc_k = (int8_T)_rtB->currentPoint;

17573


17574
    /* BusCreator: '<S10>/Bus Creator1' */

17575
    _rtB->BusCreator2.guidance.ytrack = 0.0F;

17576
    _rtB->BusCreator2.guidance.altcmd = _rtB->Gain_e;

17577
    _rtB->BusCreator2.guidance.headingcmd = 0.0F;

17578
    _rtB->BusCreator2.guidance.ascmd = _rtB->ascmd_n;

17579
    _rtB->BusCreator2.guidance.dtocenter = 0.0F;

17580
    _rtB->BusCreator2.guidance.dtoTarget = 0.0F;

17581
    _rtB->BusCreator2.guidance.currentPoint = 0;

17582
    _rtB->BusCreator2.guidance.targetpoint = _rtB->StartEngine_qgc_k;

17583


17584
    /* BusCreator: '<S10>/Bus Creator2' */

17585
    _rtB->BusCreator2.decl_qgc = AFCS_MODEL1_rtC(S)->decl_qgc;

17586
    _rtB->BusCreator2.dacl_qgc = AFCS_MODEL1_rtC(S)->dacl_qgc;

17587
    _rtB->BusCreator2.drcl_qgc = AFCS_MODEL1_rtC(S)->drcl_qgc;

17588
    _rtB->BusCreator2.nwscl_qgc = AFCS_MODEL1_rtC(S)->nwscl_qgc;

17589
    _rtB->BusCreator2.sbcl_qgc = AFCS_MODEL1_rtC(S)->sbcl_qgc;

17590
    _rtB->BusCreator2.StartEngine_qgc = _rtB->StartEngine_qgc;

17591
    _rtB->BusCreator2.TakeoffFg_qgc = _rtB->TakeoffFg_qgc;

17592
    _rtB->BusCreator2.Taxi_Fg = AFCS_MODEL1_rtC(S)->Taxi_Fg;

17593
    _rtB->BusCreator2.LockFg_qgc = AFCS_MODEL1_rtC(S)->LockFg_qgc;

17594
    _rtB->BusCreator2.INS_switch = _rtB->INS_switch;

17595
    _rtB->BusCreator2.INSIntegralClear_Fg = _rtB->INSIntegralClear_Fg;

17596
    _rtB->BusCreator2.EmergenReturnFg = 0;

17597
    _rtB->BusCreator2.EmergenLandFg = 0;

17598
    _rtB->BusCreator2.VcVtCutFg = 0;

17599
    _rtB->BusCreator2.IniposCalFg = 0;

17600


17601
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

17602
    _rtB->StartEngine_qgc_k = _rtB->BusCreator2.LockFg_qgc;

17603


17604
    /* RelationalOperator: '<S42>/Compare' incorporates:

17605
     *  Constant: '<S42>/Constant'

17606
     */

17607
    _rtB->LogicalOperator4 = (_rtB->StartEngine_qgc_k == 10);

17608


17609
    /* DataTypeConversion: '<S21>/Data Type Conversion8' */

17610
    _rtB->LockUL = (int8_T)_rtB->LogicalOperator4;

17611


17612
    /* RelationalOperator: '<S32>/Relational Operator' incorporates:

17613
     *  Constant: '<S32>/Constant2'

17614
     */

17615
    _rtB->LogicalOperator4 = (_rtB->LockUL == 0);

17616


17617
    /* Switch: '<S32>/Switch' */

17618
    _rtB->LockFlag = (int8_T)!_rtB->LogicalOperator4;

17619


17620
    /* Step: '<S16>/Step3' */

17621
    _rtB->UnaryMinus = !(ssGetTaskTime(S,0) < 80.0);

17622


17623
    /* Step: '<S16>/Step5' */

17624
    if (ssGetTaskTime(S,0) < 80.0) {

17625
      _rtB->Switch_dn = 0.0;

17626
    } else {

17627
      _rtB->Switch_dn = -1.0;

17628
    }

17629


17630
    /* End of Step: '<S16>/Step5' */

17631


17632
    /* Sum: '<S16>/Sum' */

17633
    _rtB->UnaryMinus += _rtB->Switch_dn;

17634


17635
    /* DataTypeConversion: '<S16>/Data Type Conversion7' */

17636
    TrigonometricFunction4_o = floor(_rtB->UnaryMinus);

17637
    if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))

17638
    {

17639
      TrigonometricFunction4_o = 0.0;

17640
    } else {

17641
      TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);

17642
    }

17643


17644
    _rtB->StartEngine_qgc_k = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)

17645
      (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)

17646
      (uint8_T)TrigonometricFunction4_o);

17647


17648
    /* End of DataTypeConversion: '<S16>/Data Type Conversion7' */

17649


17650
    /* RelationalOperator: '<S36>/Compare' incorporates:

17651
     *  Constant: '<S36>/Constant'

17652
     */

17653
    _rtB->LogicalOperator4 = (_rtB->StartEngine_qgc_k >= 1);

17654


17655
    /* DataTypeConversion: '<S21>/Data Type Conversion3' */

17656
    _rtB->DataTypeConversion3_j = (int8_T)_rtB->LogicalOperator4;

17657
  }

17658


17659
  /* End of Outputs for SubSystem: '<S9>/Trajectory planning,tracking' */

17660
  if (ssIsSampleHit(S, 1, 0)) {

17661
    /* Chart: '<S21>/delayblock3' */

17662
    /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock3 */

17663
    /* During: AFCS_MODEL1/GNC/Calibration/delayblock3 */

17664
    if (((uint8_T *)ssGetDWork(S, 603))[0] == 0U) {

17665
      /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock3 */

17666
      ((uint8_T *)ssGetDWork(S, 603))[0] = 1U;

17667


17668
      /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock3 */

17669
      ((uint8_T *)ssGetDWork(S, 604))[0] = AFCS_MODEL1_IN_delayblock;

17670


17671
      /* Entry 'delayblock': '<S50>:38' */

17672
      _rtB->Output_cw = 0;

17673


17674
      /* Entry Internal 'delayblock': '<S50>:38' */

17675
      /* Transition: '<S50>:164' */

17676
      if (_rtB->DataTypeConversion3_j == 0) {

17677
        /* Transition: '<S50>:160' */

17678
        ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input0;

17679


17680
        /* Entry 'Input0': '<S50>:138' */

17681
        ((int32_T *)ssGetDWork(S, 340))[0] = 0;

17682
      } else {

17683
        /* Transition: '<S50>:166' */

17684
        ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input1;

17685


17686
        /* Entry 'Input1': '<S50>:165' */

17687
        ((int32_T *)ssGetDWork(S, 340))[0] = 0;

17688
      }

17689
    } else {

17690
      /* During 'delayblock': '<S50>:38' */

17691
      switch (((uint8_T *)ssGetDWork(S, 605))[0]) {

17692
       case AFCS_MODEL1_IN_Input0:

17693
        /* During 'Input0': '<S50>:138' */

17694
        if (((int32_T *)ssGetDWork(S, 340))[0] == 20) {

17695
          /* Transition: '<S50>:137' */

17696
          ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Output0;

17697


17698
          /* Entry 'Output0': '<S50>:130' */

17699
          _rtB->Output_cw = 0;

17700
          ((int32_T *)ssGetDWork(S, 340))[0] = 0;

17701
        } else if (_rtB->DataTypeConversion3_j == 1) {

17702
          /* Transition: '<S50>:161' */

17703
          ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input1;

17704


17705
          /* Entry 'Input1': '<S50>:165' */

17706
          ((int32_T *)ssGetDWork(S, 340))[0] = 0;

17707
        } else {

17708
          ((int32_T *)ssGetDWork(S, 340))[0] = AFCS_MODEL1_addup_g(1, 20,

17709
            ((int32_T *)ssGetDWork(S, 340))[0]);

17710
        }

17711
        break;

17712


17713
       case AFCS_MODEL1_IN_Input1:

17714
        /* During 'Input1': '<S50>:165' */

17715
        if (((int32_T *)ssGetDWork(S, 340))[0] == 20) {

17716
          /* Transition: '<S50>:168' */

17717
          ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Output1;

17718


17719
          /* Entry 'Output1': '<S50>:167' */

17720
          _rtB->Output_cw = 1;

17721
          ((int32_T *)ssGetDWork(S, 340))[0] = 0;

17722
        } else if (_rtB->DataTypeConversion3_j == 0) {

17723
          /* Transition: '<S50>:162' */

17724
          ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input0;

17725


17726
          /* Entry 'Input0': '<S50>:138' */

17727
          ((int32_T *)ssGetDWork(S, 340))[0] = 0;

17728
        } else {

17729
          ((int32_T *)ssGetDWork(S, 340))[0] = AFCS_MODEL1_addup_g(1, 20,

17730
            ((int32_T *)ssGetDWork(S, 340))[0]);

17731
        }

17732
        break;

17733


17734
       case AFCS_MODEL1_IN_Output0:

17735
        /* During 'Output0': '<S50>:130' */

17736
        if (_rtB->DataTypeConversion3_j == 1) {

17737
          /* Transition: '<S50>:139' */

17738
          ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input1;

17739


17740
          /* Entry 'Input1': '<S50>:165' */

17741
          ((int32_T *)ssGetDWork(S, 340))[0] = 0;

17742
        }

17743
        break;

17744


17745
       default:

17746
        /* During 'Output1': '<S50>:167' */

17747
        if (_rtB->DataTypeConversion3_j == 0) {

17748
          /* Transition: '<S50>:170' */

17749
          ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input0;

17750


17751
          /* Entry 'Input0': '<S50>:138' */

17752
          ((int32_T *)ssGetDWork(S, 340))[0] = 0;

17753
        }

17754
        break;

17755
      }

17756
    }

17757


17758
    /* End of Chart: '<S21>/delayblock3' */

17759


17760
    /* Logic: '<S32>/Logical Operator' */

17761
    _rtB->LogicalOperator_d = !(_rtB->Output_cw != 0);

17762


17763
    /* UnitDelay: '<S31>/Unit Delay' */

17764
    _rtB->UnitDelay_e = ((int8_T *)ssGetDWork(S, 357))[0];

17765


17766
    /* UnitDelay: '<S31>/Unit Delay1' */

17767
    _rtB->UnitDelay1_p = ((int8_T *)ssGetDWork(S, 358))[0];

17768


17769
    /* UnitDelay: '<S31>/Unit Delay2' */

17770
    _rtB->UnitDelay2 = ((int8_T *)ssGetDWork(S, 359))[0];

17771


17772
    /* UnitDelay: '<S31>/Unit Delay3' */

17773
    _rtB->UnitDelay3_l = ((int8_T *)ssGetDWork(S, 360))[0];

17774


17775
    /* UnitDelay: '<S31>/Unit Delay4' */

17776
    _rtB->UnitDelay4 = ((int8_T *)ssGetDWork(S, 361))[0];

17777


17778
    /* UnitDelay: '<S31>/Unit Delay7' */

17779
    _rtB->UnitDelay7 = ((int8_T *)ssGetDWork(S, 362))[0];

17780


17781
    /* UnitDelay: '<S31>/Unit Delay5' */

17782
    _rtB->UnitDelay5 = ((int8_T *)ssGetDWork(S, 363))[0];

17783


17784
    /* MATLAB Function: '<S31>/LogicTrans' */

17785
    /* MATLAB Function 'AFCS_MODEL1/GNC/logic/LogicTrans': '<S317>:1' */

17786
    /* '<S317>:1:8' */

17787
    switch (((int8_T *)ssGetDWork(S, 514))[0]) {

17788
     case 0:

17789
      /* '<S317>:1:10' */

17790
      _rtB->fmphase = 0;

17791
      if (_rtB->UnitDelay_e == 1) {

17792
        /* '<S317>:1:11' */

17793
        /* '<S317>:1:12' */

17794
        _rtB->fmphase = 1;

17795
      }

17796
      break;

17797


17798
     case 1:

17799
      /* '<S317>:1:15' */

17800
      _rtB->fmphase = 1;

17801
      if (_rtB->UnitDelay1_p == 1) {

17802
        /* '<S317>:1:16' */

17803
        /* '<S317>:1:17' */

17804
        _rtB->fmphase = 2;

17805
      }

17806


17807
      if (_rtB->UnitDelay7 == 1) {

17808
        /* '<S317>:1:19' */

17809
        /* '<S317>:1:20' */

17810
        _rtB->fmphase = 6;

17811
      }

17812
      break;

17813


17814
     case 2:

17815
      /* '<S317>:1:23' */

17816
      _rtB->fmphase = 2;

17817
      if (_rtB->UnitDelay2 == 1) {

17818
        /* '<S317>:1:24' */

17819
        /* '<S317>:1:25' */

17820
        _rtB->fmphase = 3;

17821
      }

17822
      break;

17823


17824
     case 3:

17825
      /* '<S317>:1:28' */

17826
      _rtB->fmphase = 3;

17827
      if (_rtB->UnitDelay3_l == 1) {

17828
        /* '<S317>:1:29' */

17829
        /* '<S317>:1:30' */

17830
        _rtB->fmphase = 4;

17831
      }

17832
      break;

17833


17834
     case 4:

17835
      /* '<S317>:1:33' */

17836
      _rtB->fmphase = 4;

17837
      if (_rtB->UnitDelay4 == 1) {

17838
        /* '<S317>:1:34' */

17839
        /* '<S317>:1:35' */

17840
        _rtB->fmphase = 5;

17841
      }

17842
      break;

17843


17844
     case 5:

17845
      /* '<S317>:1:38' */

17846
      _rtB->fmphase = 5;

17847
      if (_rtB->UnitDelay5 == 1) {

17848
        /* '<S317>:1:39' */

17849
        /* '<S317>:1:40' */

17850
        _rtB->fmphase = 6;

17851
      }

17852
      break;

17853


17854
     case 6:

17855
      /* '<S317>:1:44' */

17856
      _rtB->fmphase = 6;

17857
      break;

17858


17859
     default:

17860
      /* '<S317>:1:47' */

17861
      _rtB->fmphase = 0;

17862
      break;

17863
    }

17864


17865
    /* '<S317>:1:50' */

17866
    ((int8_T *)ssGetDWork(S, 514))[0] = _rtB->fmphase;

17867


17868
    /* End of MATLAB Function: '<S31>/LogicTrans' */

17869


17870
    /* RelationalOperator: '<S32>/Relational Operator1' incorporates:

17871
     *  Constant: '<S32>/Constant3'

17872
     */

17873
    _rtB->RelationalOperator1 = (_rtB->fmphase == 0);

17874
  }

17875


17876
  if (1) {

17877
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

17878
    _rtB->StartEngine_qgc_k = _rtB->BusCreator2.StartEngine_qgc;

17879


17880
    /* RelationalOperator: '<S40>/Compare' incorporates:

17881
     *  Constant: '<S40>/Constant'

17882
     */

17883
    _rtB->LogicalOperator4 = (_rtB->StartEngine_qgc_k == 10);

17884


17885
    /* Switch: '<S21>/Switch8' incorporates:

17886
     *  Constant: '<S21>/Constant17'

17887
     *  Constant: '<S41>/Constant'

17888
     *  RelationalOperator: '<S41>/Compare'

17889
     *  SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3'

17890
     */

17891
    if (_rtB->LogicalOperator4) {

17892
      _rtB->TakeoffUL = (int8_T)(_rtB->BusCreator2.TakeoffFg_qgc == 10);

17893
    } else {

17894
      _rtB->TakeoffUL = 0;

17895
    }

17896


17897
    /* End of Switch: '<S21>/Switch8' */

17898
  }

17899


17900
  if (ssIsSampleHit(S, 1, 0)) {

17901
    /* RelationalOperator: '<S32>/Relational Operator8' incorporates:

17902
     *  Constant: '<S32>/Constant14'

17903
     */

17904
    _rtB->RelationalOperator8 = (_rtB->fmphase == 0);

17905


17906
    /* RelationalOperator: '<S35>/Compare' incorporates:

17907
     *  Constant: '<S16>/Constant7'

17908
     *  Constant: '<S35>/Constant'

17909
     */

17910
    _rtB->RelationalOperator2 = false;

17911


17912
    /* DataTypeConversion: '<S21>/Data Type Conversion2' */

17913
    _rtB->DataTypeConversion2_m = (int8_T)_rtB->RelationalOperator2;

17914


17915
    /* Chart: '<S21>/delayblock2' */

17916
    /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock2 */

17917
    /* During: AFCS_MODEL1/GNC/Calibration/delayblock2 */

17918
    if (((uint8_T *)ssGetDWork(S, 606))[0] == 0U) {

17919
      /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock2 */

17920
      ((uint8_T *)ssGetDWork(S, 606))[0] = 1U;

17921


17922
      /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock2 */

17923
      ((uint8_T *)ssGetDWork(S, 607))[0] = AFCS_MODEL1_IN_delayblock;

17924


17925
      /* Entry 'delayblock': '<S49>:38' */

17926
      _rtB->Output_b = 0;

17927


17928
      /* Entry Internal 'delayblock': '<S49>:38' */

17929
      /* Transition: '<S49>:164' */

17930
      if (_rtB->DataTypeConversion2_m == 0) {

17931
        /* Transition: '<S49>:160' */

17932
        ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input0;

17933


17934
        /* Entry 'Input0': '<S49>:138' */

17935
        ((int32_T *)ssGetDWork(S, 341))[0] = 0;

17936
      } else {

17937
        /* Transition: '<S49>:166' */

17938
        ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input1;

17939


17940
        /* Entry 'Input1': '<S49>:165' */

17941
        ((int32_T *)ssGetDWork(S, 341))[0] = 0;

17942
      }

17943
    } else {

17944
      /* During 'delayblock': '<S49>:38' */

17945
      switch (((uint8_T *)ssGetDWork(S, 608))[0]) {

17946
       case AFCS_MODEL1_IN_Input0:

17947
        /* During 'Input0': '<S49>:138' */

17948
        if (((int32_T *)ssGetDWork(S, 341))[0] == 20) {

17949
          /* Transition: '<S49>:137' */

17950
          ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Output0;

17951


17952
          /* Entry 'Output0': '<S49>:130' */

17953
          _rtB->Output_b = 0;

17954
          ((int32_T *)ssGetDWork(S, 341))[0] = 0;

17955
        } else if (_rtB->DataTypeConversion2_m == 1) {

17956
          /* Transition: '<S49>:161' */

17957
          ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input1;

17958


17959
          /* Entry 'Input1': '<S49>:165' */

17960
          ((int32_T *)ssGetDWork(S, 341))[0] = 0;

17961
        } else {

17962
          ((int32_T *)ssGetDWork(S, 341))[0] = AFCS_MODEL1_addup_l(1, 20,

17963
            ((int32_T *)ssGetDWork(S, 341))[0]);

17964
        }

17965
        break;

17966


17967
       case AFCS_MODEL1_IN_Input1:

17968
        /* During 'Input1': '<S49>:165' */

17969
        if (((int32_T *)ssGetDWork(S, 341))[0] == 20) {

17970
          /* Transition: '<S49>:168' */

17971
          ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Output1;

17972


17973
          /* Entry 'Output1': '<S49>:167' */

17974
          _rtB->Output_b = 1;

17975
          ((int32_T *)ssGetDWork(S, 341))[0] = 0;

17976
        } else if (_rtB->DataTypeConversion2_m == 0) {

17977
          /* Transition: '<S49>:162' */

17978
          ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input0;

17979


17980
          /* Entry 'Input0': '<S49>:138' */

17981
          ((int32_T *)ssGetDWork(S, 341))[0] = 0;

17982
        } else {

17983
          ((int32_T *)ssGetDWork(S, 341))[0] = AFCS_MODEL1_addup_l(1, 20,

17984
            ((int32_T *)ssGetDWork(S, 341))[0]);

17985
        }

17986
        break;

17987


17988
       case AFCS_MODEL1_IN_Output0:

17989
        /* During 'Output0': '<S49>:130' */

17990
        if (_rtB->DataTypeConversion2_m == 1) {

17991
          /* Transition: '<S49>:139' */

17992
          ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input1;

17993


17994
          /* Entry 'Input1': '<S49>:165' */

17995
          ((int32_T *)ssGetDWork(S, 341))[0] = 0;

17996
        }

17997
        break;

17998


17999
       default:

18000
        /* During 'Output1': '<S49>:167' */

18001
        if (_rtB->DataTypeConversion2_m == 0) {

18002
          /* Transition: '<S49>:170' */

18003
          ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input0;

18004


18005
          /* Entry 'Input0': '<S49>:138' */

18006
          ((int32_T *)ssGetDWork(S, 341))[0] = 0;

18007
        }

18008
        break;

18009
      }

18010
    }

18011


18012
    /* End of Chart: '<S21>/delayblock2' */

18013


18014
    /* RelationalOperator: '<S32>/Relational Operator10' incorporates:

18015
     *  Constant: '<S32>/Constant18'

18016
     */

18017
    _rtB->RelationalOperator10 = (_rtB->Output_b == 1);

18018
  }

18019


18020
  if (1) {

18021
    /* RelationalOperator: '<S32>/Relational Operator5' incorporates:

18022
     *  Constant: '<S32>/Constant12'

18023
     */

18024
    _rtB->LogicalOperator4 = (_rtB->LockFlag == 0);

18025


18026
    /* Logic: '<S32>/Logical Operator3' */

18027
    _rtB->LogicalOperator4 = (_rtB->RelationalOperator1 &&

18028
      _rtB->LogicalOperator4);

18029


18030
    /* Switch: '<S32>/Switch1' incorporates:

18031
     *  Constant: '<S32>/Constant4'

18032
     */

18033
    if (_rtB->LogicalOperator4) {

18034
      _rtB->TakeoffFlag = _rtB->TakeoffUL;

18035
    } else {

18036
      _rtB->TakeoffFlag = 0;

18037
    }

18038


18039
    /* End of Switch: '<S32>/Switch1' */

18040


18041
    /* RelationalOperator: '<S32>/Relational Operator9' incorporates:

18042
     *  Constant: '<S32>/Constant15'

18043
     */

18044
    _rtB->LogicalOperator4 = (_rtB->TakeoffFlag == 0);

18045


18046
    /* RelationalOperator: '<S32>/Relational Operator7' incorporates:

18047
     *  Constant: '<S32>/Constant13'

18048
     */

18049
    _rtB->Compare_in = (_rtB->LockFlag == 0);

18050
  }

18051


18052
  if (ssIsSampleHit(S, 1, 0)) {

18053
    /* UnitDelay: '<S32>/Unit Delay3' */

18054
    _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 364))[0];

18055


18056
    /* RelationalOperator: '<S32>/Relational Operator23' incorporates:

18057
     *  Constant: '<S32>/Constant36'

18058
     */

18059
    _rtB->RelationalOperator2 = (_rtB->UnitDelay5_b == 0);

18060


18061
    /* RelationalOperator: '<S32>/Relational Operator18' incorporates:

18062
     *  Constant: '<S32>/Constant24'

18063
     */

18064
    _rtB->RelationalOperator12 = (1 == _rtB->fmphase);

18065


18066
    /* Logic: '<S32>/Logical Operator16' */

18067
    _rtB->RelationalOperator2 = (_rtB->RelationalOperator2 &&

18068
      _rtB->RelationalOperator12);

18069


18070
    /* RelationalOperator: '<S32>/Relational Operator12' incorporates:

18071
     *  Constant: '<S32>/Constant1'

18072
     */

18073
    _rtB->RelationalOperator12 = (_rtB->fmphase == 0);

18074


18075
    /* RelationalOperator: '<S32>/Relational Operator19' incorporates:

18076
     *  Constant: '<S32>/Constant32'

18077
     */

18078
    _rtB->RelationalOperator19 = (_rtB->fmphase == 6);

18079


18080
    /* RelationalOperator: '<S32>/Relational Operator20' incorporates:

18081
     *  Constant: '<S32>/Constant33'

18082
     */

18083
    _rtB->RelationalOperator20 = (_rtB->fmphase == 5);

18084
  }

18085


18086
  if (1) {

18087
    /* Logic: '<S32>/Logical Operator4' */

18088
    _rtB->LogicalOperator4 = (_rtB->LogicalOperator4 && _rtB->Compare_in &&

18089
      _rtB->RelationalOperator8 && _rtB->RelationalOperator10);

18090


18091
    /* Switch: '<S32>/Switch5' */

18092
    _rtB->Swcheck = (int8_T)_rtB->LogicalOperator4;

18093


18094
    /* RelationalOperator: '<S32>/Relational Operator11' incorporates:

18095
     *  Constant: '<S32>/Constant19'

18096
     */

18097
    _rtB->Compare_in = (_rtB->Swcheck == 1);

18098


18099
    /* Logic: '<S32>/Logical Operator5' */

18100
    _rtB->Compare_in = (_rtB->LogicalOperator_d || _rtB->Compare_in);

18101


18102
    /* DataTypeConversion: '<S32>/Data Type Conversion' */

18103
    _rtB->SWAUTO = (int8_T)_rtB->Compare_in;

18104


18105
    /* RelationalOperator: '<S32>/Relational Operator21' incorporates:

18106
     *  Constant: '<S32>/Constant34'

18107
     */

18108
    _rtB->Compare_in = (_rtB->mh_j < 5.0F);

18109


18110
    /* Logic: '<S32>/Logical Operator12' */

18111
    _rtB->Compare_in = (_rtB->RelationalOperator20 && _rtB->Compare_in);

18112


18113
    /* DataTypeConversion: '<S32>/Data Type Conversion7' */

18114
    _rtB->DataTypeConversion7_k = (int8_T)_rtB->Compare_in;

18115


18116
    /* RelationalOperator: '<S32>/Relational Operator22' incorporates:

18117
     *  Constant: '<S32>/Constant35'

18118
     */

18119
    _rtB->Compare_in = (_rtB->Nz > 1.3F);

18120


18121
    /* DataTypeConversion: '<S32>/Data Type Conversion6' */

18122
    _rtB->DataTypeConversion6_o = (int8_T)_rtB->Compare_in;

18123
  }

18124


18125
  if (ssIsSampleHit(S, 1, 0)) {

18126
    /* Chart: '<S32>/wowcmp1' */

18127
    /* Gateway: AFCS_MODEL1/GNC/setup/wowcmp1 */

18128
    /* During: AFCS_MODEL1/GNC/setup/wowcmp1 */

18129
    if (((uint8_T *)ssGetDWork(S, 524))[0] == 0U) {

18130
      /* Entry: AFCS_MODEL1/GNC/setup/wowcmp1 */

18131
      ((uint8_T *)ssGetDWork(S, 524))[0] = 1U;

18132


18133
      /* Entry Internal: AFCS_MODEL1/GNC/setup/wowcmp1 */

18134
      ((uint8_T *)ssGetDWork(S, 525))[0] = AFCS_MODEL1_IN_Enlowfg;

18135


18136
      /* Entry 'Enlowfg': '<S331>:38' */

18137
      _rtB->EnLowFlag_n = 0;

18138


18139
      /* Entry Internal 'Enlowfg': '<S331>:38' */

18140
      /* Transition: '<S331>:131' */

18141
      ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_count;

18142


18143
      /* Entry 'count': '<S331>:138' */

18144
      ((int32_T *)ssGetDWork(S, 296))[0] = 0;

18145
    } else {

18146
      /* During 'Enlowfg': '<S331>:38' */

18147
      switch (((uint8_T *)ssGetDWork(S, 526))[0]) {

18148
       case AFCS_MODEL1_IN_count:

18149
        /* During 'count': '<S331>:138' */

18150
        if (((int32_T *)ssGetDWork(S, 296))[0] == 200) {

18151
          /* Transition: '<S331>:137' */

18152
          ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_enableFG;

18153


18154
          /* Entry 'enableFG': '<S331>:130' */

18155
          _rtB->EnLowFlag_n = 0;

18156
        } else {

18157
          ((int32_T *)ssGetDWork(S, 296))[0] = AFCS_MODEL1_addup_p

18158
            (_rtB->DataTypeConversion7_k > 0, 200, ((int32_T *)ssGetDWork(S, 296))

18159
             [0]);

18160
        }

18161
        break;

18162


18163
       case AFCS_MODEL1_IN_enableFG:

18164
        /* During 'enableFG': '<S331>:130' */

18165
        if (_rtB->DataTypeConversion6_o > 0) {

18166
          /* Transition: '<S331>:140' */

18167
          ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_enableFG1;

18168


18169
          /* Entry 'enableFG1': '<S331>:139' */

18170
          _rtB->EnLowFlag_n = 1;

18171
        }

18172
        break;

18173


18174
       default:

18175
        /* During 'enableFG1': '<S331>:139' */

18176
        break;

18177
      }

18178
    }

18179


18180
    /* End of Chart: '<S32>/wowcmp1' */

18181


18182
    /* Logic: '<S32>/Logical Operator6' */

18183
    _rtB->RelationalOperator_l = (_rtB->RelationalOperator2 ||

18184
      _rtB->RelationalOperator12 || _rtB->RelationalOperator19 ||

18185
      (_rtB->EnLowFlag_n != 0));

18186


18187
    /* Switch: '<S32>/Switch6' */

18188
    _rtB->Switch6_j = (int8_T)_rtB->RelationalOperator_l;

18189


18190
    /* DataTypeConversion: '<S16>/Data Type Conversion' incorporates:

18191
     *  Constant: '<S16>/Constant'

18192
     */

18193
    _rtB->Divide_n = 0.0F;

18194


18195
    /* Sum: '<S43>/Sum3' */

18196
    _rtB->Divide_n -= AFCS_MODEL1_rtC(S)->Gain;

18197


18198
    /* Product: '<S43>/Divide' */

18199
    _rtB->Divide_n /= AFCS_MODEL1_rtC(S)->Sum_dn;

18200


18201
    /* Saturate: '<S21>/Saturation' */

18202
    if (_rtB->Divide_n > 1.0F) {

18203
      _rtB->Saturation_l = 1.0F;

18204
    } else if (_rtB->Divide_n < -1.0F) {

18205
      _rtB->Saturation_l = -1.0F;

18206
    } else {

18207
      _rtB->Saturation_l = _rtB->Divide_n;

18208
    }

18209


18210
    /* End of Saturate: '<S21>/Saturation' */

18211


18212
    /* DataTypeConversion: '<S16>/Data Type Conversion1' incorporates:

18213
     *  Constant: '<S16>/Constant1'

18214
     */

18215
    _rtB->Divide_n = 0.0F;

18216


18217
    /* Sum: '<S44>/Sum3' */

18218
    _rtB->Divide_n -= AFCS_MODEL1_rtC(S)->Gain_l;

18219


18220
    /* Product: '<S44>/Divide' */

18221
    _rtB->Divide_n /= AFCS_MODEL1_rtC(S)->Sum_es;

18222


18223
    /* Saturate: '<S21>/Saturation1' */

18224
    if (_rtB->Divide_n > 1.0F) {

18225
      _rtB->Saturation1_k = 1.0F;

18226
    } else if (_rtB->Divide_n < -1.0F) {

18227
      _rtB->Saturation1_k = -1.0F;

18228
    } else {

18229
      _rtB->Saturation1_k = _rtB->Divide_n;

18230
    }

18231


18232
    /* End of Saturate: '<S21>/Saturation1' */

18233


18234
    /* DataTypeConversion: '<S16>/Data Type Conversion3' incorporates:

18235
     *  Constant: '<S16>/Constant3'

18236
     */

18237
    _rtB->Divide_n = 0.0F;

18238


18239
    /* Sum: '<S45>/Sum3' */

18240
    _rtB->Divide_n -= AFCS_MODEL1_rtC(S)->Gain_m;

18241


18242
    /* Product: '<S45>/Divide' */

18243
    _rtB->Divide_n /= AFCS_MODEL1_rtC(S)->Sum_i;

18244


18245
    /* Saturate: '<S21>/Saturation2' */

18246
    if (_rtB->Divide_n > 1.0F) {

18247
      _rtB->Saturation2 = 1.0F;

18248
    } else if (_rtB->Divide_n < -1.0F) {

18249
      _rtB->Saturation2 = -1.0F;

18250
    } else {

18251
      _rtB->Saturation2 = _rtB->Divide_n;

18252
    }

18253


18254
    /* End of Saturate: '<S21>/Saturation2' */

18255


18256
    /* Sum: '<S47>/Sum' incorporates:

18257
     *  Constant: '<S21>/Constant3'

18258
     *  Constant: '<S21>/Constant6'

18259
     */

18260
    _rtB->Sum_h = 800.0F;

18261
  }

18262


18263
  if (1) {

18264
    /* Switch: '<S21>/Switch' */

18265
    _rtB->Switch_lo[0] = _rtB->Saturation_l;

18266
    _rtB->Switch_lo[1] = _rtB->Saturation1_k;

18267
    _rtB->Switch_lo[2] = _rtB->Saturation2;

18268


18269
    /* Product: '<S47>/Divide' incorporates:

18270
     *  Constant: '<S21>/Constant6'

18271
     *  DataTypeConversion: '<S16>/Data Type Conversion2'

18272
     *  Sum: '<S16>/Sum3'

18273
     *  Sum: '<S47>/Sum3'

18274
     *  Switch: '<S21>/Switch'

18275
     */

18276
    DXp = -1100.0F / _rtB->Sum_h;

18277


18278
    /* Saturate: '<S21>/Saturation3' incorporates:

18279
     *  Switch: '<S21>/Switch'

18280
     */

18281
    if (DXp > 1.0F) {

18282
      _rtB->Switch_lo[3] = 1.0F;

18283
    } else if (DXp < 0.0F) {

18284
      _rtB->Switch_lo[3] = 0.0F;

18285
    } else {

18286
      _rtB->Switch_lo[3] = DXp;

18287
    }

18288


18289
    /* End of Saturate: '<S21>/Saturation3' */

18290


18291
    /* Gain: '<S26>/Gain' */

18292
    _rtB->Gain_e = 0.3F * _rtB->Switch_lo[3];

18293


18294
    /* Saturate: '<S26>/Saturation' */

18295
    if (_rtB->Gain_e > 0.3F) {

18296
      _rtB->Saturation_n = 0.3F;

18297
    } else if (_rtB->Gain_e < 0.0F) {

18298
      _rtB->Saturation_n = 0.0F;

18299
    } else {

18300
      _rtB->Saturation_n = _rtB->Gain_e;

18301
    }

18302


18303
    /* End of Saturate: '<S26>/Saturation' */

18304


18305
    /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */

18306
    {

18307
      SimStruct *rts = ssGetSFunction(S, 167);

18308
      sfcnOutputs(rts, 0);

18309
      if (ssGetErrorStatus(rts) != (NULL))

18310
        return;

18311
    }

18312


18313
    /* Gain: '<S26>/Gain1' */

18314
    _rtB->Gain_e = -20.0F * _rtB->GeneratedSFunction15;

18315


18316
    /* Saturate: '<S26>/Saturation1' */

18317
    if (_rtB->Gain_e > 30.0F) {

18318
      _rtB->Saturation1_f = 30.0F;

18319
    } else if (_rtB->Gain_e < -30.0F) {

18320
      _rtB->Saturation1_f = -30.0F;

18321
    } else {

18322
      _rtB->Saturation1_f = _rtB->Gain_e;

18323
    }

18324


18325
    /* End of Saturate: '<S26>/Saturation1' */

18326


18327
    /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */

18328
    {

18329
      SimStruct *rts = ssGetSFunction(S, 168);

18330
      sfcnOutputs(rts, 0);

18331
      if (ssGetErrorStatus(rts) != (NULL))

18332
        return;

18333
    }

18334


18335
    /* Gain: '<S26>/Gain2' */

18336
    _rtB->Gain_e = -20.0F * _rtB->GeneratedSFunction15_i;

18337


18338
    /* Saturate: '<S26>/Saturation2' */

18339
    if (_rtB->Gain_e > 30.0F) {

18340
      _rtB->Saturation2_o = 30.0F;

18341
    } else if (_rtB->Gain_e < -30.0F) {

18342
      _rtB->Saturation2_o = -30.0F;

18343
    } else {

18344
      _rtB->Saturation2_o = _rtB->Gain_e;

18345
    }

18346


18347
    /* End of Saturate: '<S26>/Saturation2' */

18348


18349
    /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */

18350
    {

18351
      SimStruct *rts = ssGetSFunction(S, 169);

18352
      sfcnOutputs(rts, 0);

18353
      if (ssGetErrorStatus(rts) != (NULL))

18354
        return;

18355
    }

18356


18357
    /* Gain: '<S26>/Gain3' */

18358
    _rtB->Gain_e = -20.0F * _rtB->GeneratedSFunction15_e;

18359


18360
    /* Saturate: '<S26>/Saturation3' */

18361
    if (_rtB->Gain_e > 30.0F) {

18362
      _rtB->Saturation3_f = 30.0F;

18363
    } else if (_rtB->Gain_e < -30.0F) {

18364
      _rtB->Saturation3_f = -30.0F;

18365
    } else {

18366
      _rtB->Saturation3_f = _rtB->Gain_e;

18367
    }

18368


18369
    /* End of Saturate: '<S26>/Saturation3' */

18370


18371
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

18372
    _rtB->decl_qgc = _rtB->BusCreator2.decl_qgc;

18373


18374
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

18375
    _rtB->dacl_qgc = _rtB->BusCreator2.dacl_qgc;

18376


18377
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

18378
    _rtB->drcl_qgc = _rtB->BusCreator2.drcl_qgc;

18379


18380
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

18381
    _rtB->nwscl_qgc = _rtB->BusCreator2.nwscl_qgc;

18382


18383
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

18384
    _rtB->sbcl_qgc = _rtB->BusCreator2.sbcl_qgc;

18385


18386
    /* DataTypeConversion: '<S1>/Data Type Conversion5' incorporates:

18387
     *  Inport: '<Root>/CMD_HdotC'

18388
     */

18389
    _rtB->CMD_Hdotc = *((const real32_T **)ssGetInputPortSignalPtrs(S, 4))[0];

18390


18391
    /* DataTypeConversion: '<S24>/Data Type Conversion57' */

18392
    _rtB->IndexAL = _rtB->IndexAL_k;

18393


18394
    /* DataTypeConversion: '<S24>/Data Type Conversion3' */

18395
    _rtB->DtoStart = _rtB->DtoStart_b;

18396
  }

18397


18398
  if (ssIsSampleHit(S, 1, 0)) {

18399
    /* Switch: '<S32>/Switch7' */

18400
    _rtB->Switch7_n = (int8_T)_rtB->RelationalOperator_l;

18401


18402
    /* RelationalOperator: '<S38>/Compare' incorporates:

18403
     *  Constant: '<S16>/Constant5'

18404
     *  Constant: '<S38>/Constant'

18405
     */

18406
    _rtB->RelationalOperator_l = false;

18407


18408
    /* DataTypeConversion: '<S21>/Data Type Conversion6' */

18409
    _rtB->DataTypeConversion6_f = (int8_T)_rtB->RelationalOperator_l;

18410


18411
    /* Chart: '<S21>/delayblock5' */

18412
    /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock5 */

18413
    /* During: AFCS_MODEL1/GNC/Calibration/delayblock5 */

18414
    if (((uint8_T *)ssGetDWork(S, 597))[0] == 0U) {

18415
      /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock5 */

18416
      ((uint8_T *)ssGetDWork(S, 597))[0] = 1U;

18417


18418
      /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock5 */

18419
      ((uint8_T *)ssGetDWork(S, 598))[0] = AFCS_MODEL1_IN_delayblock;

18420


18421
      /* Entry 'delayblock': '<S52>:38' */

18422
      _rtB->Output_o = 0;

18423


18424
      /* Entry Internal 'delayblock': '<S52>:38' */

18425
      /* Transition: '<S52>:164' */

18426
      if (_rtB->DataTypeConversion6_f == 0) {

18427
        /* Transition: '<S52>:160' */

18428
        ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input0;

18429


18430
        /* Entry 'Input0': '<S52>:138' */

18431
        ((int32_T *)ssGetDWork(S, 338))[0] = 0;

18432
      } else {

18433
        /* Transition: '<S52>:166' */

18434
        ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input1;

18435


18436
        /* Entry 'Input1': '<S52>:165' */

18437
        ((int32_T *)ssGetDWork(S, 338))[0] = 0;

18438
      }

18439
    } else {

18440
      /* During 'delayblock': '<S52>:38' */

18441
      switch (((uint8_T *)ssGetDWork(S, 599))[0]) {

18442
       case AFCS_MODEL1_IN_Input0:

18443
        /* During 'Input0': '<S52>:138' */

18444
        if (((int32_T *)ssGetDWork(S, 338))[0] == 20) {

18445
          /* Transition: '<S52>:137' */

18446
          ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Output0;

18447


18448
          /* Entry 'Output0': '<S52>:130' */

18449
          _rtB->Output_o = 0;

18450
          ((int32_T *)ssGetDWork(S, 338))[0] = 0;

18451
        } else if (_rtB->DataTypeConversion6_f == 1) {

18452
          /* Transition: '<S52>:161' */

18453
          ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input1;

18454


18455
          /* Entry 'Input1': '<S52>:165' */

18456
          ((int32_T *)ssGetDWork(S, 338))[0] = 0;

18457
        } else {

18458
          ((int32_T *)ssGetDWork(S, 338))[0] = AFCS_MODEL1_addup_k(1, 20,

18459
            ((int32_T *)ssGetDWork(S, 338))[0]);

18460
        }

18461
        break;

18462


18463
       case AFCS_MODEL1_IN_Input1:

18464
        /* During 'Input1': '<S52>:165' */

18465
        if (((int32_T *)ssGetDWork(S, 338))[0] == 20) {

18466
          /* Transition: '<S52>:168' */

18467
          ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Output1;

18468


18469
          /* Entry 'Output1': '<S52>:167' */

18470
          _rtB->Output_o = 1;

18471
          ((int32_T *)ssGetDWork(S, 338))[0] = 0;

18472
        } else if (_rtB->DataTypeConversion6_f == 0) {

18473
          /* Transition: '<S52>:162' */

18474
          ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input0;

18475


18476
          /* Entry 'Input0': '<S52>:138' */

18477
          ((int32_T *)ssGetDWork(S, 338))[0] = 0;

18478
        } else {

18479
          ((int32_T *)ssGetDWork(S, 338))[0] = AFCS_MODEL1_addup_k(1, 20,

18480
            ((int32_T *)ssGetDWork(S, 338))[0]);

18481
        }

18482
        break;

18483


18484
       case AFCS_MODEL1_IN_Output0:

18485
        /* During 'Output0': '<S52>:130' */

18486
        if (_rtB->DataTypeConversion6_f == 1) {

18487
          /* Transition: '<S52>:139' */

18488
          ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input1;

18489


18490
          /* Entry 'Input1': '<S52>:165' */

18491
          ((int32_T *)ssGetDWork(S, 338))[0] = 0;

18492
        }

18493
        break;

18494


18495
       default:

18496
        /* During 'Output1': '<S52>:167' */

18497
        if (_rtB->DataTypeConversion6_f == 0) {

18498
          /* Transition: '<S52>:170' */

18499
          ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input0;

18500


18501
          /* Entry 'Input0': '<S52>:138' */

18502
          ((int32_T *)ssGetDWork(S, 338))[0] = 0;

18503
        }

18504
        break;

18505
      }

18506
    }

18507


18508
    /* End of Chart: '<S21>/delayblock5' */

18509


18510
    /* Logic: '<S32>/Logical Operator13' incorporates:

18511
     *  Constant: '<S13>/Constant9'

18512
     */

18513
    _rtB->RelationalOperator_l = true;

18514


18515
    /* Logic: '<S32>/Logical Operator14' */

18516
    _rtB->RelationalOperator_l = !_rtB->RelationalOperator_l;

18517


18518
    /* DataTypeConversion: '<S32>/Data Type Conversion8' */

18519
    _rtB->GPSFail = (int8_T)_rtB->RelationalOperator_l;

18520


18521
    /* RelationalOperator: '<S32>/Relational Operator3' incorporates:

18522
     *  Constant: '<S32>/Constant7'

18523
     */

18524
    _rtB->RelationalOperator_l = (_rtB->fmphase == 1);

18525


18526
    /* Switch: '<S32>/Switch3' incorporates:

18527
     *  Constant: '<S32>/Constant8'

18528
     */

18529
    if (_rtB->RelationalOperator_l) {

18530
      /* Logic: '<S32>/Logical Operator15' */

18531
      _rtB->LogicalOperator15 = ((_rtB->Output_o != 0) || (_rtB->GPSFail != 0));

18532


18533
      /* DataTypeConversion: '<S32>/Data Type Conversion9' */

18534
      _rtB->DataTypeConversion9_c = (int8_T)_rtB->LogicalOperator15;

18535
      _rtB->Switch3 = _rtB->DataTypeConversion9_c;

18536
    } else {

18537
      _rtB->Switch3 = 0;

18538
    }

18539


18540
    /* End of Switch: '<S32>/Switch3' */

18541


18542
    /* RelationalOperator: '<S32>/Relational Operator16' incorporates:

18543
     *  Constant: '<S32>/Constant11'

18544
     */

18545
    _rtB->RelationalOperator16 = (_rtB->fmphase == 5);

18546


18547
    /* RelationalOperator: '<S39>/Compare' incorporates:

18548
     *  Constant: '<S16>/Constant4'

18549
     *  Constant: '<S39>/Constant'

18550
     */

18551
    _rtB->RelationalOperator_l = false;

18552


18553
    /* DataTypeConversion: '<S21>/Data Type Conversion7' */

18554
    _rtB->DataTypeConversion7_n = (int8_T)_rtB->RelationalOperator_l;

18555


18556
    /* Chart: '<S21>/delayblock7' */

18557
    /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock7 */

18558
    /* During: AFCS_MODEL1/GNC/Calibration/delayblock7 */

18559
    if (((uint8_T *)ssGetDWork(S, 591))[0] == 0U) {

18560
      /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock7 */

18561
      ((uint8_T *)ssGetDWork(S, 591))[0] = 1U;

18562


18563
      /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock7 */

18564
      ((uint8_T *)ssGetDWork(S, 592))[0] = AFCS_MODEL1_IN_delayblock;

18565


18566
      /* Entry 'delayblock': '<S54>:38' */

18567
      _rtB->Output_i = 0;

18568


18569
      /* Entry Internal 'delayblock': '<S54>:38' */

18570
      /* Transition: '<S54>:164' */

18571
      if (_rtB->DataTypeConversion7_n == 0) {

18572
        /* Transition: '<S54>:160' */

18573
        ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input0;

18574


18575
        /* Entry 'Input0': '<S54>:138' */

18576
        ((int32_T *)ssGetDWork(S, 336))[0] = 0;

18577
      } else {

18578
        /* Transition: '<S54>:166' */

18579
        ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input1;

18580


18581
        /* Entry 'Input1': '<S54>:165' */

18582
        ((int32_T *)ssGetDWork(S, 336))[0] = 0;

18583
      }

18584
    } else {

18585
      /* During 'delayblock': '<S54>:38' */

18586
      switch (((uint8_T *)ssGetDWork(S, 593))[0]) {

18587
       case AFCS_MODEL1_IN_Input0:

18588
        /* During 'Input0': '<S54>:138' */

18589
        if (((int32_T *)ssGetDWork(S, 336))[0] == 20) {

18590
          /* Transition: '<S54>:137' */

18591
          ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Output0;

18592


18593
          /* Entry 'Output0': '<S54>:130' */

18594
          _rtB->Output_i = 0;

18595
          ((int32_T *)ssGetDWork(S, 336))[0] = 0;

18596
        } else if (_rtB->DataTypeConversion7_n == 1) {

18597
          /* Transition: '<S54>:161' */

18598
          ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input1;

18599


18600
          /* Entry 'Input1': '<S54>:165' */

18601
          ((int32_T *)ssGetDWork(S, 336))[0] = 0;

18602
        } else {

18603
          ((int32_T *)ssGetDWork(S, 336))[0] = AFCS_MODEL1_addup_h(1, 20,

18604
            ((int32_T *)ssGetDWork(S, 336))[0]);

18605
        }

18606
        break;

18607


18608
       case AFCS_MODEL1_IN_Input1:

18609
        /* During 'Input1': '<S54>:165' */

18610
        if (((int32_T *)ssGetDWork(S, 336))[0] == 20) {

18611
          /* Transition: '<S54>:168' */

18612
          ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Output1;

18613


18614
          /* Entry 'Output1': '<S54>:167' */

18615
          _rtB->Output_i = 1;

18616
          ((int32_T *)ssGetDWork(S, 336))[0] = 0;

18617
        } else if (_rtB->DataTypeConversion7_n == 0) {

18618
          /* Transition: '<S54>:162' */

18619
          ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input0;

18620


18621
          /* Entry 'Input0': '<S54>:138' */

18622
          ((int32_T *)ssGetDWork(S, 336))[0] = 0;

18623
        } else {

18624
          ((int32_T *)ssGetDWork(S, 336))[0] = AFCS_MODEL1_addup_h(1, 20,

18625
            ((int32_T *)ssGetDWork(S, 336))[0]);

18626
        }

18627
        break;

18628


18629
       case AFCS_MODEL1_IN_Output0:

18630
        /* During 'Output0': '<S54>:130' */

18631
        if (_rtB->DataTypeConversion7_n == 1) {

18632
          /* Transition: '<S54>:139' */

18633
          ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input1;

18634


18635
          /* Entry 'Input1': '<S54>:165' */

18636
          ((int32_T *)ssGetDWork(S, 336))[0] = 0;

18637
        }

18638
        break;

18639


18640
       default:

18641
        /* During 'Output1': '<S54>:167' */

18642
        if (_rtB->DataTypeConversion7_n == 0) {

18643
          /* Transition: '<S54>:170' */

18644
          ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input0;

18645


18646
          /* Entry 'Input0': '<S54>:138' */

18647
          ((int32_T *)ssGetDWork(S, 336))[0] = 0;

18648
        }

18649
        break;

18650
      }

18651
    }

18652


18653
    /* End of Chart: '<S21>/delayblock7' */

18654


18655
    /* RelationalOperator: '<S32>/Relational Operator6' incorporates:

18656
     *  Constant: '<S32>/Constant28'

18657
     */

18658
    _rtB->RelationalOperator6 = (_rtB->Output_i == 1);

18659
  }

18660


18661
  if (1) {

18662
    /* DataTypeConversion: '<S293>/Data Type Conversion3' */

18663
    _rtB->PSDref = _rtB->Switch_oz[3];

18664


18665
    /* DataTypeConversion: '<S293>/Data Type Conversion8' */

18666
    _rtB->href = _rtB->Switch_oz[0];

18667


18668
    /* DataTypeConversion: '<S293>/Data Type Conversion4' */

18669
    _rtB->drange = _rtB->Switch_oz[4];

18670


18671
    /* DataTypeConversion: '<S293>/Data Type Conversion2' */

18672
    _rtB->xrw = _rtB->Switch_oz[1];

18673


18674
    /* RelationalOperator: '<S32>/Relational Operator15' incorporates:

18675
     *  Constant: '<S32>/Constant10'

18676
     */

18677
    _rtB->Compare_in = (_rtB->LockFlag == 0);

18678
  }

18679


18680
  if (ssIsSampleHit(S, 1, 0)) {

18681
    /* Constant: '<S13>/Constant23' */

18682
    _rtB->EnLowFlag = 0;

18683


18684
    /* Chart: '<S21>/delayblock4' */

18685
    /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock4 */

18686
    /* During: AFCS_MODEL1/GNC/Calibration/delayblock4 */

18687
    if (((uint8_T *)ssGetDWork(S, 600))[0] == 0U) {

18688
      /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock4 */

18689
      ((uint8_T *)ssGetDWork(S, 600))[0] = 1U;

18690


18691
      /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock4 */

18692
      ((uint8_T *)ssGetDWork(S, 601))[0] = AFCS_MODEL1_IN_delayblock;

18693


18694
      /* Entry 'delayblock': '<S51>:38' */

18695
      _rtB->Output_c = 0;

18696


18697
      /* Entry Internal 'delayblock': '<S51>:38' */

18698
      /* Transition: '<S51>:164' */

18699
      if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 0) {

18700
        /* Transition: '<S51>:160' */

18701
        ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input0;

18702


18703
        /* Entry 'Input0': '<S51>:138' */

18704
        ((int32_T *)ssGetDWork(S, 339))[0] = 0;

18705
      } else {

18706
        /* Transition: '<S51>:166' */

18707
        ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input1;

18708


18709
        /* Entry 'Input1': '<S51>:165' */

18710
        ((int32_T *)ssGetDWork(S, 339))[0] = 0;

18711
      }

18712
    } else {

18713
      /* During 'delayblock': '<S51>:38' */

18714
      switch (((uint8_T *)ssGetDWork(S, 602))[0]) {

18715
       case AFCS_MODEL1_IN_Input0:

18716
        /* During 'Input0': '<S51>:138' */

18717
        if (((int32_T *)ssGetDWork(S, 339))[0] == 20) {

18718
          /* Transition: '<S51>:137' */

18719
          ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Output0;

18720


18721
          /* Entry 'Output0': '<S51>:130' */

18722
          _rtB->Output_c = 0;

18723
          ((int32_T *)ssGetDWork(S, 339))[0] = 0;

18724
        } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 1) {

18725
          /* Transition: '<S51>:161' */

18726
          ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input1;

18727


18728
          /* Entry 'Input1': '<S51>:165' */

18729
          ((int32_T *)ssGetDWork(S, 339))[0] = 0;

18730
        } else {

18731
          ((int32_T *)ssGetDWork(S, 339))[0] = AFCS_MODEL1_addup_i(1, 20,

18732
            ((int32_T *)ssGetDWork(S, 339))[0]);

18733
        }

18734
        break;

18735


18736
       case AFCS_MODEL1_IN_Input1:

18737
        /* During 'Input1': '<S51>:165' */

18738
        if (((int32_T *)ssGetDWork(S, 339))[0] == 20) {

18739
          /* Transition: '<S51>:168' */

18740
          ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Output1;

18741


18742
          /* Entry 'Output1': '<S51>:167' */

18743
          _rtB->Output_c = 1;

18744
          ((int32_T *)ssGetDWork(S, 339))[0] = 0;

18745
        } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 0) {

18746
          /* Transition: '<S51>:162' */

18747
          ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input0;

18748


18749
          /* Entry 'Input0': '<S51>:138' */

18750
          ((int32_T *)ssGetDWork(S, 339))[0] = 0;

18751
        } else {

18752
          ((int32_T *)ssGetDWork(S, 339))[0] = AFCS_MODEL1_addup_i(1, 20,

18753
            ((int32_T *)ssGetDWork(S, 339))[0]);

18754
        }

18755
        break;

18756


18757
       case AFCS_MODEL1_IN_Output0:

18758
        /* During 'Output0': '<S51>:130' */

18759
        if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 1) {

18760
          /* Transition: '<S51>:139' */

18761
          ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input1;

18762


18763
          /* Entry 'Input1': '<S51>:165' */

18764
          ((int32_T *)ssGetDWork(S, 339))[0] = 0;

18765
        }

18766
        break;

18767


18768
       default:

18769
        /* During 'Output1': '<S51>:167' */

18770
        if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 0) {

18771
          /* Transition: '<S51>:170' */

18772
          ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input0;

18773


18774
          /* Entry 'Input0': '<S51>:138' */

18775
          ((int32_T *)ssGetDWork(S, 339))[0] = 0;

18776
        }

18777
        break;

18778
      }

18779
    }

18780


18781
    /* End of Chart: '<S21>/delayblock4' */

18782


18783
    /* RelationalOperator: '<S32>/Relational Operator4' incorporates:

18784
     *  Constant: '<S32>/Constant5'

18785
     */

18786
    _rtB->RelationalOperator_l = (_rtB->fmphase == 3);

18787


18788
    /* RelationalOperator: '<S32>/Relational Operator2' incorporates:

18789
     *  Constant: '<S32>/Constant9'

18790
     */

18791
    _rtB->RelationalOperator2 = (_rtB->fmphase == 4);

18792


18793
    /* Logic: '<S32>/Logical Operator9' */

18794
    _rtB->RelationalOperator_l = (_rtB->RelationalOperator_l ||

18795
      _rtB->RelationalOperator2);

18796


18797
    /* Switch: '<S32>/Switch2' incorporates:

18798
     *  Constant: '<S32>/Constant6'

18799
     */

18800
    if (_rtB->RelationalOperator_l) {

18801
      _rtB->EmlandFg = _rtB->Output_c;

18802
    } else {

18803
      _rtB->EmlandFg = 0;

18804
    }

18805


18806
    /* End of Switch: '<S32>/Switch2' */

18807
  }

18808


18809
  if (1) {

18810
    /* Logic: '<S32>/Logical Operator10' */

18811
    _rtB->Compare_in = (_rtB->Compare_in && _rtB->RelationalOperator16 &&

18812
                        _rtB->RelationalOperator6);

18813


18814
    /* Switch: '<S32>/Switch4' */

18815
    _rtB->GoaroundFg = (int8_T)_rtB->Compare_in;

18816


18817
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

18818
    _rtB->Gain_e = _rtB->BusCreator2.guidance.altcmd;

18819


18820
    /* Saturate: '<S31>/Saturation2' */

18821
    if (_rtB->Gain_e > 5000.0F) {

18822
      _rtB->Saturation2_g = 5000.0F;

18823
    } else if (_rtB->Gain_e < 10.0F) {

18824
      _rtB->Saturation2_g = 10.0F;

18825
    } else {

18826
      _rtB->Saturation2_g = _rtB->Gain_e;

18827
    }

18828


18829
    /* End of Saturate: '<S31>/Saturation2' */

18830


18831
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

18832
    _rtB->ascmd = _rtB->BusCreator2.guidance.ascmd;

18833


18834
    /* MATLAB Function: '<S31>/MATLAB Function1' */

18835
    /* MATLAB Function 'AFCS_MODEL1/GNC/logic/MATLAB Function1': '<S318>:1' */

18836
    /* '<S318>:1:3' */

18837
    _rtB->y = (real32_T)atan(_rtB->ascmd / 9.8F) * 1.2F * 57.3F;

18838


18839
    /* Saturate: '<S31>/Saturation' */

18840
    if (_rtB->y > 30.0F) {

18841
      _rtB->Saturation_h = 30.0F;

18842
    } else if (_rtB->y < -30.0F) {

18843
      _rtB->Saturation_h = -30.0F;

18844
    } else {

18845
      _rtB->Saturation_h = _rtB->y;

18846
    }

18847


18848
    /* End of Saturate: '<S31>/Saturation' */

18849


18850
    /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */

18851
    _rtB->targetpoint = _rtB->BusCreator2.guidance.targetpoint;

18852


18853
    /* MATLAB Function: '<S320>/MATLAB Function2' */

18854
    /* MATLAB Function 'AFCS_MODEL1/GNC/logic/Subsystem2/MATLAB Function2': '<S328>:1' */

18855
    if (_rtB->targetpoint < 2) {

18856
      /* '<S328>:1:3' */

18857
      /* '<S328>:1:4' */

18858
      _rtB->y_p = 2;

18859


18860
      /* 不能小于2,前两个点的高度是0 */

18861
    } else {

18862
      i_1 = _rtB->NumPoint - 3;

18863
      if (i_1 < -128) {

18864
        i_1 = -128;

18865
      }

18866


18867
      if (_rtB->targetpoint > i_1) {

18868
        /* '<S328>:1:5' */

18869
        /* '<S328>:1:6' */

18870
        i_1 = _rtB->NumPoint - 3;

18871
        if (i_1 < -128) {

18872
          i_1 = -128;

18873
        }

18874


18875
        _rtB->y_p = (int8_T)i_1;

18876


18877
        /* 程序里k从1开始，最后两个点的高度是0，倒数第三个点有进场标志，所以不能超过倒数第四个点。 */

18878
      } else {

18879
        /* '<S328>:1:8' */

18880
        _rtB->y_p = _rtB->targetpoint;

18881
      }

18882
    }

18883


18884
    /* End of MATLAB Function: '<S320>/MATLAB Function2' */

18885


18886
    /* SwitchCase: '<S31>/Switch Case' */

18887
    rtAction = -1;

18888
    if (ssIsMajorTimeStep(S)) {

18889
      switch (_rtB->fmphase) {

18890
       case 0:

18891
        rtAction = 0;

18892
        break;

18893


18894
       case 1:

18895
        rtAction = 1;

18896
        break;

18897


18898
       case 2:

18899
        rtAction = 2;

18900
        break;

18901


18902
       case 3:

18903
        rtAction = 3;

18904
        break;

18905


18906
       case 4:

18907
        rtAction = 4;

18908
        break;

18909


18910
       case 5:

18911
        rtAction = 5;

18912
        break;

18913


18914
       case 6:

18915
        rtAction = 6;

18916
        break;

18917
      }

18918


18919
      ((int8_T *)ssGetDWork(S, 505))[0] = rtAction;

18920
    } else {

18921
      rtAction = ((int8_T *)ssGetDWork(S, 505))[0];

18922
    }

18923


18924
    switch (rtAction) {

18925
     case 0:

18926
      /* Outputs for IfAction SubSystem: '<S31>/Pre-Takeoff' incorporates:

18927
       *  ActionPort: '<S319>/Action Port'

18928
       */

18929
      if (ssIsSampleHit(S, 1, 0)) {

18930
        /* Chart: '<S319>/Pre_Takeoff' */

18931
        /* Gateway: AFCS_MODEL1/GNC/logic/Pre-Takeoff/Pre_Takeoff */

18932
        /* During: AFCS_MODEL1/GNC/logic/Pre-Takeoff/Pre_Takeoff */

18933
        if (((uint8_T *)ssGetDWork(S, 580))[0] == 0U) {

18934
          /* Entry: AFCS_MODEL1/GNC/logic/Pre-Takeoff/Pre_Takeoff */

18935
          ((uint8_T *)ssGetDWork(S, 580))[0] = 1U;

18936


18937
          /* Entry Internal: AFCS_MODEL1/GNC/logic/Pre-Takeoff/Pre_Takeoff */

18938
          ((uint8_T *)ssGetDWork(S, 581))[0] = AFCS_MODEL1_IN_Pre_Takeoff;

18939


18940
          /* Entry 'Pre_Takeoff': '<S327>:38' */

18941
          _rtB->configure_cu = 1;

18942
          _rtB->apthL_p = 1;

18943
          _rtB->aplonL_fl = 1;

18944
          _rtB->aplatL_a1 = 1;

18945
          _rtB->apyawL_e = 1;

18946
          _rtB->apsbL_i = 1;

18947
          _rtB->apnwsL_e = 1;

18948
          _rtB->apbrkL_p = 2;

18949
          _rtB->thrustcmd_c = 0.0F;

18950
          _rtB->vtcmd_ld = 0.0F;

18951
          _rtB->nzcmd_c = 0.0F;

18952
          _rtB->nycmd_d = 0.0F;

18953
          _rtB->phiLcmd_e = 0.0F;

18954
          _rtB->thetaLcmd_n = 0.0F;

18955
          _rtB->psiLcmd_a = 0.0F;

18956
          _rtB->gamacmd_a = 0.0F;

18957
          _rtB->hcmd_jv = 0.0F;

18958
          _rtB->psdcmd_j = 0.0F;

18959
          _rtB->yrwcmd_g = 0.0F;

18960
          _rtB->fmpretotakeoff = 0;

18961
          _rtB->StartEmNav_m = 0;

18962


18963
          /* Entry Internal 'Pre_Takeoff': '<S327>:38' */

18964
          /* Transition: '<S327>:36' */

18965
          ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_LockState;

18966


18967
          /* Entry 'LockState': '<S327>:28' */

18968
          ((int32_T *)ssGetDWork(S, 330))[0] = 0;

18969
          _rtB->apcheck_m = 0;

18970
          _rtB->apfix_o = 1;

18971
        } else {

18972
          /* During 'Pre_Takeoff': '<S327>:38' */

18973
          _rtB->K_i = 1;

18974
          switch (((uint8_T *)ssGetDWork(S, 582))[0]) {

18975
           case AFCS_MODEL1_IN_LockState:

18976
            /* During 'LockState': '<S327>:28' */

18977
            if (((int32_T *)ssGetDWork(S, 330))[0] == 5) {

18978
              /* Transition: '<S327>:30' */

18979
              ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_StickCheck;

18980


18981
              /* Entry 'StickCheck': '<S327>:29' */

18982
              _rtB->apfix_o = 0;

18983
              _rtB->apcheck_m = 1;

18984
              ((int32_T *)ssGetDWork(S, 329))[0] = 0;

18985
              ((int32_T *)ssGetDWork(S, 328))[0] = 0;

18986
            } else {

18987
              i_1 = 1 - _rtB->LockFlag;

18988
              if (i_1 > 127) {

18989
                i_1 = 127;

18990
              }

18991


18992
              ((int32_T *)ssGetDWork(S, 330))[0] = AFCS_MODEL1_addup_ke((int8_T)

18993
                i_1, 5, ((int32_T *)ssGetDWork(S, 330))[0]);

18994
            }

18995
            break;

18996


18997
           case AFCS_MODEL1_IN_StickCheck:

18998
            /* During 'StickCheck': '<S327>:29' */

18999
            if (((int32_T *)ssGetDWork(S, 329))[0] == 5) {

19000
              /* Transition: '<S327>:43' */

19001
              ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_TransTakeoff;

19002


19003
              /* Entry 'TransTakeoff': '<S327>:42' */

19004
              _rtB->apfix_o = 0;

19005
              _rtB->apcheck_m = 0;

19006


19007
              /* Entry Internal 'TransTakeoff': '<S327>:42' */

19008
              /* Transition: '<S327>:92' */

19009
              if (_rtB->GPSFail == 0) {

19010
                /* Transition: '<S327>:90' */

19011
                _rtB->fmpretotakeoff = 1;

19012
              } else {

19013
                /* Transition: '<S327>:91' */

19014
                _rtB->fmpretotakeoff = 0;

19015
              }

19016
            } else if (((int32_T *)ssGetDWork(S, 328))[0] == 5) {

19017
              /* Transition: '<S327>:79' */

19018
              ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_LockState;

19019


19020
              /* Entry 'LockState': '<S327>:28' */

19021
              ((int32_T *)ssGetDWork(S, 330))[0] = 0;

19022
              _rtB->apcheck_m = 0;

19023
              _rtB->apfix_o = 1;

19024
            } else {

19025
              ((int32_T *)ssGetDWork(S, 329))[0] = AFCS_MODEL1_addup_ke

19026
                (_rtB->TakeoffFlag, 5, ((int32_T *)ssGetDWork(S, 329))[0]);

19027
              ((int32_T *)ssGetDWork(S, 328))[0] = AFCS_MODEL1_addup_ke

19028
                (_rtB->LockFlag, 5, ((int32_T *)ssGetDWork(S, 328))[0]);

19029
            }

19030
            break;

19031


19032
           default:

19033
            /* During 'TransTakeoff': '<S327>:42' */

19034
            break;

19035
          }

19036
        }

19037


19038
        /* End of Chart: '<S319>/Pre_Takeoff' */

19039
      }

19040


19041
      if (1) {

19042
        /* SignalConversion: '<S319>/Signal Conversion' */

19043
        _rtB->configure = _rtB->configure_cu;

19044


19045
        /* SignalConversion: '<S319>/Signal Conversion' */

19046
        _rtB->vtcmd = _rtB->vtcmd_ld;

19047


19048
        /* SignalConversion: '<S319>/Signal Conversion' */

19049
        _rtB->nzcmd = _rtB->nzcmd_c;

19050


19051
        /* SignalConversion: '<S319>/Signal Conversion' */

19052
        _rtB->nycmd = _rtB->nycmd_d;

19053


19054
        /* SignalConversion: '<S319>/Signal Conversion' */

19055
        _rtB->phiLcmd = _rtB->phiLcmd_e;

19056


19057
        /* SignalConversion: '<S319>/Signal Conversion' */

19058
        _rtB->thetaLcmd = _rtB->thetaLcmd_n;

19059


19060
        /* SignalConversion: '<S319>/Signal Conversion' */

19061
        _rtB->psiLcmd = _rtB->psiLcmd_a;

19062


19063
        /* SignalConversion: '<S319>/Signal Conversion' */

19064
        _rtB->gamacmd = _rtB->gamacmd_a;

19065


19066
        /* SignalConversion: '<S319>/Signal Conversion' */

19067
        _rtB->hcmd = _rtB->hcmd_jv;

19068


19069
        /* SignalConversion: '<S319>/Signal Conversion' */

19070
        _rtB->psdcmd = _rtB->psdcmd_j;

19071


19072
        /* SignalConversion: '<S319>/Signal Conversion' */

19073
        _rtB->yrwcmd = _rtB->yrwcmd_g;

19074


19075
        /* SignalConversion: '<S319>/Signal Conversion' */

19076
        _rtB->apthL = _rtB->apthL_p;

19077


19078
        /* SignalConversion: '<S319>/Signal Conversion' */

19079
        _rtB->apcheck = _rtB->apcheck_m;

19080


19081
        /* SignalConversion: '<S319>/Signal Conversion' */

19082
        _rtB->apfix = _rtB->apfix_o;

19083


19084
        /* SignalConversion: '<S319>/Signal Conversion' */

19085
        _rtB->K = _rtB->K_i;

19086


19087
        /* SignalConversion: '<S319>/Signal Conversion' */

19088
        _rtB->StartEmNav = _rtB->StartEmNav_m;

19089


19090
        /* SignalConversion: '<S319>/Signal Conversion' */

19091
        _rtB->aplonL = _rtB->aplonL_fl;

19092


19093
        /* SignalConversion: '<S319>/Signal Conversion' */

19094
        _rtB->aplatL = _rtB->aplatL_a1;

19095


19096
        /* SignalConversion: '<S319>/Signal Conversion' */

19097
        _rtB->apyawL = _rtB->apyawL_e;

19098


19099
        /* SignalConversion: '<S319>/Signal Conversion' */

19100
        _rtB->apsbL = _rtB->apsbL_i;

19101


19102
        /* SignalConversion: '<S319>/Signal Conversion' */

19103
        _rtB->apnwsL = _rtB->apnwsL_e;

19104


19105
        /* SignalConversion: '<S319>/Signal Conversion' */

19106
        _rtB->apbrkL = _rtB->apbrkL_p;

19107


19108
        /* SignalConversion: '<S319>/Signal Conversion' */

19109
        _rtB->thrustcmd = _rtB->thrustcmd_c;

19110
      }

19111


19112
      /* End of Outputs for SubSystem: '<S31>/Pre-Takeoff' */

19113
      break;

19114


19115
     case 1:

19116
      /* Outputs for IfAction SubSystem: '<S31>/Takeoff' incorporates:

19117
       *  ActionPort: '<S321>/Action Port'

19118
       */

19119
      if (ssIsSampleHit(S, 1, 0)) {

19120
        /* Chart: '<S321>/Takeoff' incorporates:

19121
         *  Constant: '<S17>/Constant9'

19122
         */

19123
        /* Gateway: AFCS_MODEL1/GNC/logic/Takeoff/Takeoff */

19124
        /* During: AFCS_MODEL1/GNC/logic/Takeoff/Takeoff */

19125
        if (((uint8_T *)ssGetDWork(S, 576))[0] == 0U) {

19126
          /* Entry: AFCS_MODEL1/GNC/logic/Takeoff/Takeoff */

19127
          ((uint8_T *)ssGetDWork(S, 576))[0] = 1U;

19128


19129
          /* Entry Internal: AFCS_MODEL1/GNC/logic/Takeoff/Takeoff */

19130
          ((uint8_T *)ssGetDWork(S, 577))[0] = AFCS_MODEL1_IN_Takeoff;

19131


19132
          /* Entry 'Takeoff': '<S329>:38' */

19133
          _rtB->apsbL_l1 = 1;

19134
          _rtB->vtcmd_e = 0.0F;

19135
          _rtB->nzcmd_iw = 0.0F;

19136
          _rtB->nycmd_b = 0.0F;

19137
          _rtB->gamacmd_d = 0.0F;

19138
          _rtB->hcmd_kk = 0.0F;

19139
          _rtB->psdcmd_b = 0.0F;

19140
          _rtB->yrwcmd_m = 0.0F;

19141
          _rtB->apfix_hs = 0;

19142
          _rtB->apcheck_i = 0;

19143
          _rtB->fmTOtoClimb = 0;

19144
          _rtB->fmTOtoTaxi = 0;

19145
          _rtB->K_cw = 1;

19146
          _rtB->StartEmNav_f = 0;

19147
          _rtB->LiftWheelFg = 0;

19148
          _rtB->AbortTakeoffFg = 0;

19149


19150
          /* Entry Internal 'Takeoff': '<S329>:38' */

19151
          /* Transition: '<S329>:36' */

19152
          ((uint8_T *)ssGetDWork(S, 578))[0] = AFCS_MODEL1_IN_NormalTakeoff;

19153


19154
          /* Entry 'NormalTakeoff': '<S329>:136' */

19155
          _rtB->configure_f = 1;

19156
          _rtB->apthL_i2 = 1;

19157
          _rtB->aplonL_ik = 1;

19158
          _rtB->aplatL_d = 1;

19159
          _rtB->apyawL_g = 1;

19160
          _rtB->apnwsL_g = 1;

19161


19162
          /* 由2改为1 */

19163
          _rtB->apbrkL_h = 1;

19164
          _rtB->thrustcmd_g = 0.85F;

19165
          _rtB->phiLcmd_h = 0.0F;

19166
          _rtB->thetaLcmd_ae = 0.0F;

19167
          _rtB->psiLcmd_p = 0.0F;

19168
          ((int32_T *)ssGetDWork(S, 326))[0] = 0;

19169


19170
          /* Entry Internal 'NormalTakeoff': '<S329>:136' */

19171
          /* Transition: '<S329>:195' */

19172
          ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_Initial;

19173


19174
          /* Entry 'Initial': '<S329>:193' */

19175
          ((int32_T *)ssGetDWork(S, 327))[0] = 0;

19176
        } else {

19177
          /* During 'Takeoff': '<S329>:38' */

19178
          if (((uint8_T *)ssGetDWork(S, 578))[0] != AFCS_MODEL1_IN_AbortTakeoff)

19179
          {

19180
            AFCS_MODEL1_NormalTakeoff(S);

19181
          } else {

19182
            /* During 'AbortTakeoff': '<S329>:152' */

19183
          }

19184
        }

19185


19186
        /* End of Chart: '<S321>/Takeoff' */

19187
      }

19188


19189
      if (1) {

19190
        /* SignalConversion: '<S321>/Signal Conversion' */

19191
        _rtB->configure = _rtB->configure_f;

19192


19193
        /* SignalConversion: '<S321>/Signal Conversion' */

19194
        _rtB->vtcmd = _rtB->vtcmd_e;

19195


19196
        /* SignalConversion: '<S321>/Signal Conversion' */

19197
        _rtB->nzcmd = _rtB->nzcmd_iw;

19198


19199
        /* SignalConversion: '<S321>/Signal Conversion' */

19200
        _rtB->nycmd = _rtB->nycmd_b;

19201


19202
        /* SignalConversion: '<S321>/Signal Conversion' */

19203
        _rtB->phiLcmd = _rtB->phiLcmd_h;

19204


19205
        /* SignalConversion: '<S321>/Signal Conversion' */

19206
        _rtB->thetaLcmd = _rtB->thetaLcmd_ae;

19207


19208
        /* SignalConversion: '<S321>/Signal Conversion' */

19209
        _rtB->psiLcmd = _rtB->psiLcmd_p;

19210


19211
        /* SignalConversion: '<S321>/Signal Conversion' */

19212
        _rtB->gamacmd = _rtB->gamacmd_d;

19213


19214
        /* SignalConversion: '<S321>/Signal Conversion' */

19215
        _rtB->hcmd = _rtB->hcmd_kk;

19216


19217
        /* SignalConversion: '<S321>/Signal Conversion' */

19218
        _rtB->psdcmd = _rtB->psdcmd_b;

19219


19220
        /* SignalConversion: '<S321>/Signal Conversion' */

19221
        _rtB->yrwcmd = _rtB->yrwcmd_m;

19222


19223
        /* SignalConversion: '<S321>/Signal Conversion' */

19224
        _rtB->apthL = _rtB->apthL_i2;

19225


19226
        /* SignalConversion: '<S321>/Signal Conversion' */

19227
        _rtB->apcheck = _rtB->apcheck_i;

19228


19229
        /* SignalConversion: '<S321>/Signal Conversion' */

19230
        _rtB->apfix = _rtB->apfix_hs;

19231


19232
        /* SignalConversion: '<S321>/Signal Conversion' */

19233
        _rtB->K = _rtB->K_cw;

19234


19235
        /* SignalConversion: '<S321>/Signal Conversion' */

19236
        _rtB->StartEmNav = _rtB->StartEmNav_f;

19237


19238
        /* SignalConversion: '<S321>/Signal Conversion' */

19239
        _rtB->aplonL = _rtB->aplonL_ik;

19240


19241
        /* SignalConversion: '<S321>/Signal Conversion' */

19242
        _rtB->aplatL = _rtB->aplatL_d;

19243


19244
        /* SignalConversion: '<S321>/Signal Conversion' */

19245
        _rtB->apyawL = _rtB->apyawL_g;

19246


19247
        /* SignalConversion: '<S321>/Signal Conversion' */

19248
        _rtB->apsbL = _rtB->apsbL_l1;

19249


19250
        /* SignalConversion: '<S321>/Signal Conversion' */

19251
        _rtB->apnwsL = _rtB->apnwsL_g;

19252


19253
        /* SignalConversion: '<S321>/Signal Conversion' */

19254
        _rtB->apbrkL = _rtB->apbrkL_h;

19255


19256
        /* SignalConversion: '<S321>/Signal Conversion' */

19257
        _rtB->thrustcmd = _rtB->thrustcmd_g;

19258
      }

19259


19260
      /* End of Outputs for SubSystem: '<S31>/Takeoff' */

19261
      break;

19262


19263
     case 2:

19264
      /* Outputs for IfAction SubSystem: '<S31>/Climb' incorporates:

19265
       *  ActionPort: '<S314>/Action Port'

19266
       */

19267
      if (ssIsSampleHit(S, 1, 0)) {

19268
        /* Chart: '<S314>/Climb2' incorporates:

19269
         *  Constant: '<S17>/Constant10'

19270
         *  Constant: '<S17>/Constant15'

19271
         *  Constant: '<S17>/Constant16'

19272
         *  Constant: '<S17>/Constant6'

19273
         *  Constant: '<S17>/Constant9'

19274
         */

19275
        /* Gateway: AFCS_MODEL1/GNC/logic/Climb/Climb2 */

19276
        /* During: AFCS_MODEL1/GNC/logic/Climb/Climb2 */

19277
        if (((uint8_T *)ssGetDWork(S, 567))[0] == 0U) {

19278
          /* Entry: AFCS_MODEL1/GNC/logic/Climb/Climb2 */

19279
          ((uint8_T *)ssGetDWork(S, 567))[0] = 1U;

19280


19281
          /* Entry Internal: AFCS_MODEL1/GNC/logic/Climb/Climb2 */

19282
          ((uint8_T *)ssGetDWork(S, 568))[0] = AFCS_MODEL1_IN_Climb;

19283


19284
          /* Entry 'Climb': '<S324>:38' */

19285
          _rtB->configure_o = 1;

19286
          _rtB->apsbL_ex = 1;

19287
          _rtB->apnwsL_ku = 1;

19288
          _rtB->apbrkL_b = 1;

19289
          _rtB->thrustcmd_k = 0.85F;

19290
          _rtB->vtcmd_h1 = 0.0F;

19291
          _rtB->nzcmd_o = 0.0F;

19292
          _rtB->nycmd_n = 0.0F;

19293
          _rtB->psiLcmd_j = 0.0F;

19294
          _rtB->gamacmd_j = 0.0F;

19295
          _rtB->hcmd_eq = _rtB->href;

19296
          _rtB->apfix_h5 = 0;

19297
          _rtB->apcheck_c = 0;

19298
          _rtB->fmClimbtoCruise = 0;

19299
          _rtB->K_j = 2;

19300
          _rtB->StartEmNav_a2 = 0;

19301


19302
          /* Entry Internal 'Climb': '<S324>:38' */

19303
          /* Transition: '<S324>:95' */

19304
          ((uint8_T *)ssGetDWork(S, 569))[0] = AFCS_MODEL1_IN_PositionControl;

19305


19306
          /* Entry 'PositionControl': '<S324>:29' */

19307
          _rtB->apthL_fb = 1;

19308
          _rtB->aplonL_b = 2;

19309
          _rtB->phiLcmd_o = 0.0F;

19310
          _rtB->thetaLcmd_g = 5.0F;

19311
          _rtB->yrwcmd_a = 0.0F;

19312
          ((int32_T *)ssGetDWork(S, 320))[0] = 0;

19313


19314
          /* Entry Internal 'PositionControl': '<S324>:29' */

19315
          ((uint8_T *)ssGetDWork(S, 570))[0] = 1U;

19316
          ((uint8_T *)ssGetDWork(S, 572))[0] = 1U;

19317


19318
          /* Entry Internal 'XYPositionControl': '<S324>:131' */

19319
          /* Transition: '<S324>:133' */

19320
          ((uint8_T *)ssGetDWork(S, 571))[0] = AFCS_MODEL1_IN_PositionControl1;

19321


19322
          /* Entry 'PositionControl1': '<S324>:135' */

19323
          /* Entry Internal 'PositionControl1': '<S324>:135' */

19324
          /* Transition: '<S324>:215' */

19325
          ((uint8_T *)ssGetDWork(S, 573))[0] = AFCS_MODEL1_IN_YrwControl;

19326


19327
          /* Entry Internal 'YrwControl': '<S324>:195' */

19328
          /* Transition: '<S324>:196' */

19329
          ((uint8_T *)ssGetDWork(S, 574))[0] = AFCS_MODEL1_IN_phiLcontrol;

19330


19331
          /* Entry 'phiLcontrol': '<S324>:199' */

19332
          _rtB->aplatL_b = 5;

19333
          _rtB->apyawL_n = 1;

19334
          _rtB->psdcmd_o = 0.0F;

19335
          ((int32_T *)ssGetDWork(S, 321))[0] = 0;

19336
          ((uint8_T *)ssGetDWork(S, 575))[0] = 1U;

19337
        } else {

19338
          /* During 'Climb': '<S324>:38' */

19339
          if (((uint8_T *)ssGetDWork(S, 569))[0] ==

19340
              AFCS_MODEL1_IN_PositionControl) {

19341
            /* During 'PositionControl': '<S324>:29' */

19342
            if (((int32_T *)ssGetDWork(S, 320))[0] == 5) {

19343
              /* Transition: '<S324>:43' */

19344
              /* Exit Internal 'PositionControl': '<S324>:29' */

19345
              ((uint8_T *)ssGetDWork(S, 575))[0] = 0U;

19346


19347
              /* Exit Internal 'XYPositionControl': '<S324>:131' */

19348
              /* Exit Internal 'PositionControl1': '<S324>:135' */

19349
              ((uint8_T *)ssGetDWork(S, 573))[0] =

19350
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

19351


19352
              /* Exit Internal 'YrwControl': '<S324>:195' */

19353
              ((uint8_T *)ssGetDWork(S, 574))[0] =

19354
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

19355
              ((uint8_T *)ssGetDWork(S, 571))[0] =

19356
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

19357
              ((uint8_T *)ssGetDWork(S, 572))[0] = 0U;

19358
              ((uint8_T *)ssGetDWork(S, 570))[0] = 0U;

19359
              ((uint8_T *)ssGetDWork(S, 569))[0] = AFCS_MODEL1_IN_TransCruise;

19360


19361
              /* Entry 'TransCruise': '<S324>:42' */

19362
              _rtB->fmClimbtoCruise = 1;

19363
              _rtB->K_j = 2;

19364
            } else {

19365
              _rtB->hcmd_eq = _rtB->href;

19366


19367
              /* %Climb1FG=((drange<single(200.))&&(GPSFail==int8(0))); */

19368
              ((int32_T *)ssGetDWork(S, 320))[0] = AFCS_MODEL1_addup_j

19369
                ((_rtB->drange < 400.0F) && (_rtB->GPSFail == 0), 5, ((int32_T *)

19370
                  ssGetDWork(S, 320))[0]);

19371


19372
              /* During 'AltitudeControl': '<S324>:115' */

19373
              /* Transition: '<S324>:118' */

19374
              if (_rtB->href - _rtB->mh_j < -50.0F) {

19375
                /* Transition: '<S324>:116' */

19376
                _rtB->aplonL_b = 3;

19377
                _rtB->gamacmd_j = -5.0F;

19378
              } else if (_rtB->href - _rtB->mh_j > 50.0F) {

19379
                /* Transition: '<S324>:117' */

19380
                _rtB->aplonL_b = 3;

19381
                _rtB->gamacmd_j = 5.0F;

19382
              } else {

19383
                /* Transition: '<S324>:123' */

19384
                _rtB->aplonL_b = 4;

19385
                _rtB->gamacmd_j = 0.0F;

19386
              }

19387


19388
              /* During 'XYPositionControl': '<S324>:131' */

19389
              /* During 'PositionControl1': '<S324>:135' */

19390
              if (((uint8_T *)ssGetDWork(S, 573))[0] ==

19391
                  AFCS_MODEL1_IN_PhiControl) {

19392
                /* During 'PhiControl': '<S324>:187' */

19393
                if (_rtB->GPSFail == 0) {

19394
                  /* Transition: '<S324>:211' */

19395
                  ((uint8_T *)ssGetDWork(S, 573))[0] = AFCS_MODEL1_IN_YrwControl;

19396


19397
                  /* Entry Internal 'YrwControl': '<S324>:195' */

19398
                  /* Transition: '<S324>:196' */

19399
                  ((uint8_T *)ssGetDWork(S, 574))[0] =

19400
                    AFCS_MODEL1_IN_phiLcontrol;

19401


19402
                  /* Entry 'phiLcontrol': '<S324>:199' */

19403
                  _rtB->aplatL_b = 5;

19404
                  _rtB->apyawL_n = 1;

19405
                  _rtB->psdcmd_o = 0.0F;

19406
                  ((int32_T *)ssGetDWork(S, 321))[0] = 0;

19407
                } else {

19408
                  /* Transition: '<S324>:189' */

19409
                  if (_rtB->mh_j > 30.0F) {

19410
                    /* Transition: '<S324>:191' */

19411
                    _rtB->phiLcmd_o = 30.0F;

19412
                  } else {

19413
                    /* Transition: '<S324>:194' */

19414
                    _rtB->phiLcmd_o = 0.0F;

19415
                  }

19416
                }

19417
              } else {

19418
                /* During 'YrwControl': '<S324>:195' */

19419
                if (_rtB->GPSFail == 1) {

19420
                  /* Transition: '<S324>:193' */

19421
                  /* Exit Internal 'YrwControl': '<S324>:195' */

19422
                  ((uint8_T *)ssGetDWork(S, 574))[0] =

19423
                    AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

19424
                  ((uint8_T *)ssGetDWork(S, 573))[0] = AFCS_MODEL1_IN_PhiControl;

19425


19426
                  /* Entry 'PhiControl': '<S324>:187' */

19427
                  _rtB->aplatL_b = 2;

19428
                  _rtB->apyawL_n = 1;

19429
                } else if (((uint8_T *)ssGetDWork(S, 574))[0] ==

19430
                           AFCS_MODEL1_IN_YrwControl_o) {

19431
                  /* During 'YrwControl': '<S324>:205' */

19432
                  /* Transition: '<S324>:207' */

19433
                  if (_rtB->yrw - _rtB->yrwcmd_a > 50.0F) {

19434
                    /* Transition: '<S324>:209' */

19435
                    _rtB->aplatL_b = 4;

19436
                    _rtB->psdcmd_o = -10.0F;

19437
                    _rtB->phiLcmd_o = 0.0F;

19438
                  } else if (_rtB->yrw - _rtB->yrwcmd_a < -50.0F) {

19439
                    /* Transition: '<S324>:210' */

19440
                    _rtB->aplatL_b = 4;

19441
                    _rtB->psdcmd_o = 10.0F;

19442
                    _rtB->phiLcmd_o = 0.0F;

19443
                  } else {

19444
                    /* Transition: '<S324>:208' */

19445
                    _rtB->aplatL_b = 5;

19446
                    _rtB->yrwcmd_a = 0.0F;

19447
                    _rtB->psdcmd_o = 0.0F;

19448
                    _rtB->phiLcmd_o = 0.0F;

19449
                  }

19450
                } else {

19451
                  /* During 'phiLcontrol': '<S324>:199' */

19452
                  if ((((int32_T *)ssGetDWork(S, 321))[0] == 5) && (_rtB->SWAUTO

19453
                       == 1)) {

19454
                    /* Transition: '<S324>:197' */

19455
                    ((uint8_T *)ssGetDWork(S, 574))[0] =

19456
                      AFCS_MODEL1_IN_YrwControl_o;

19457
                  } else {

19458
                    ((int32_T *)ssGetDWork(S, 321))[0] = AFCS_MODEL1_addup_j

19459
                      ((real32_T)fabs(AFCS_MODEL1_res180(_rtB->psdcmd_o -

19460
                         _rtB->psd)) < 15.0F, 5, ((int32_T *)ssGetDWork(S, 321))

19461
                       [0]);

19462


19463
                    /* Transition: '<S324>:201' */

19464
                    if (AFCS_MODEL1_res180(_rtB->psdcmd_o - _rtB->psd) > 20.0F)

19465
                    {

19466
                      /* Transition: '<S324>:202' */

19467
                      _rtB->aplatL_b = 2;

19468
                      _rtB->phiLcmd_o = 30.0F;

19469
                    } else {

19470
                      if (AFCS_MODEL1_res180(_rtB->psdcmd_o - _rtB->psd) <

19471
                          -20.0F) {

19472
                        /* Transition: '<S324>:204' */

19473
                        _rtB->aplatL_b = 2;

19474
                        _rtB->phiLcmd_o = -30.0F;

19475
                      } else {

19476
                        /* Transition: '<S324>:203' */

19477
                        _rtB->aplatL_b = 4;

19478
                        _rtB->psdcmd_o = 0.0F;

19479
                        _rtB->phiLcmd_o = 0.0F;

19480
                      }

19481
                    }

19482
                  }

19483
                }

19484
              }

19485


19486
              /* During 'ThrustControl': '<S324>:175' */

19487
              /* Transition: '<S324>:177' */

19488
              if (_rtB->Vc_p > 45.0F) {

19489
                /* Transition: '<S324>:182' */

19490
                _rtB->apthL_fb = 3;

19491
                _rtB->vtcmd_h1 = 45.0F;

19492
              } else {

19493
                /* Transition: '<S324>:183' */

19494
              }

19495
            }

19496
          } else {

19497
            /* During 'TransCruise': '<S324>:42' */

19498
          }

19499
        }

19500


19501
        /* End of Chart: '<S314>/Climb2' */

19502
      }

19503


19504
      if (1) {

19505
        /* SignalConversion: '<S314>/Signal Conversion' */

19506
        _rtB->configure = _rtB->configure_o;

19507


19508
        /* SignalConversion: '<S314>/Signal Conversion' */

19509
        _rtB->vtcmd = _rtB->vtcmd_h1;

19510


19511
        /* SignalConversion: '<S314>/Signal Conversion' */

19512
        _rtB->nzcmd = _rtB->nzcmd_o;

19513


19514
        /* SignalConversion: '<S314>/Signal Conversion' */

19515
        _rtB->nycmd = _rtB->nycmd_n;

19516


19517
        /* SignalConversion: '<S314>/Signal Conversion' */

19518
        _rtB->phiLcmd = _rtB->phiLcmd_o;

19519


19520
        /* SignalConversion: '<S314>/Signal Conversion' */

19521
        _rtB->thetaLcmd = _rtB->thetaLcmd_g;

19522


19523
        /* SignalConversion: '<S314>/Signal Conversion' */

19524
        _rtB->psiLcmd = _rtB->psiLcmd_j;

19525


19526
        /* SignalConversion: '<S314>/Signal Conversion' */

19527
        _rtB->gamacmd = _rtB->gamacmd_j;

19528


19529
        /* SignalConversion: '<S314>/Signal Conversion' */

19530
        _rtB->hcmd = _rtB->hcmd_eq;

19531


19532
        /* SignalConversion: '<S314>/Signal Conversion' */

19533
        _rtB->psdcmd = _rtB->psdcmd_o;

19534


19535
        /* SignalConversion: '<S314>/Signal Conversion' */

19536
        _rtB->yrwcmd = _rtB->yrwcmd_a;

19537


19538
        /* SignalConversion: '<S314>/Signal Conversion' */

19539
        _rtB->apthL = _rtB->apthL_fb;

19540


19541
        /* SignalConversion: '<S314>/Signal Conversion' */

19542
        _rtB->apcheck = _rtB->apcheck_c;

19543


19544
        /* SignalConversion: '<S314>/Signal Conversion' */

19545
        _rtB->apfix = _rtB->apfix_h5;

19546


19547
        /* SignalConversion: '<S314>/Signal Conversion' */

19548
        _rtB->K = _rtB->K_j;

19549


19550
        /* SignalConversion: '<S314>/Signal Conversion' */

19551
        _rtB->StartEmNav = _rtB->StartEmNav_a2;

19552


19553
        /* SignalConversion: '<S314>/Signal Conversion' */

19554
        _rtB->aplonL = _rtB->aplonL_b;

19555


19556
        /* SignalConversion: '<S314>/Signal Conversion' */

19557
        _rtB->aplatL = _rtB->aplatL_b;

19558


19559
        /* SignalConversion: '<S314>/Signal Conversion' */

19560
        _rtB->apyawL = _rtB->apyawL_n;

19561


19562
        /* SignalConversion: '<S314>/Signal Conversion' */

19563
        _rtB->apsbL = _rtB->apsbL_ex;

19564


19565
        /* SignalConversion: '<S314>/Signal Conversion' */

19566
        _rtB->apnwsL = _rtB->apnwsL_ku;

19567


19568
        /* SignalConversion: '<S314>/Signal Conversion' */

19569
        _rtB->apbrkL = _rtB->apbrkL_b;

19570


19571
        /* SignalConversion: '<S314>/Signal Conversion' */

19572
        _rtB->thrustcmd = _rtB->thrustcmd_k;

19573
      }

19574


19575
      /* End of Outputs for SubSystem: '<S31>/Climb' */

19576
      break;

19577


19578
     case 3:

19579
      /* Outputs for IfAction SubSystem: '<S31>/Cruise' incorporates:

19580
       *  ActionPort: '<S315>/Action Port'

19581
       */

19582
      if (ssIsSampleHit(S, 1, 0)) {

19583
        /* Chart: '<S315>/Cruise' incorporates:

19584
         *  Constant: '<S17>/Constant15'

19585
         *  Constant: '<S17>/Constant16'

19586
         *  Constant: '<S17>/Constant6'

19587
         *  Inport: '<Root>/AT_MODE'

19588
         *  Inport: '<Root>/CMD_PhiC'

19589
         *  Inport: '<Root>/DV'

19590
         *  Inport: '<Root>/YD_MODE'

19591
         */

19592
        /* Gateway: AFCS_MODEL1/GNC/logic/Cruise/Cruise */

19593
        /* During: AFCS_MODEL1/GNC/logic/Cruise/Cruise */

19594
        if (((uint8_T *)ssGetDWork(S, 557))[0] == 0U) {

19595
          /* Entry: AFCS_MODEL1/GNC/logic/Cruise/Cruise */

19596
          ((uint8_T *)ssGetDWork(S, 557))[0] = 1U;

19597


19598
          /* Entry Internal: AFCS_MODEL1/GNC/logic/Cruise/Cruise */

19599
          ((uint8_T *)ssGetDWork(S, 558))[0] = AFCS_MODEL1_IN_Cruise;

19600


19601
          /* Entry 'Cruise': '<S325>:38' */

19602
          _rtB->configure_j = 1;

19603


19604
          /* 滚转角控制模式 */

19605
          _rtB->apyawL_mn = 1;

19606
          _rtB->apsbL_g = 1;

19607
          _rtB->apnwsL_o = 1;

19608
          _rtB->apbrkL_a = 1;

19609
          _rtB->thrustcmd_f = 0.0F;

19610
          _rtB->vtcmd_l = 40.0F;

19611
          _rtB->nzcmd_n = 0.0F;

19612
          _rtB->nycmd_l = 0.0F;

19613
          _rtB->thetaLcmd_a = 0.0F;

19614
          _rtB->psiLcmd_e = 0.0F;

19615
          _rtB->psdcmd_ik = 0.0F;

19616
          _rtB->yrwcmd_f = 0.0F;

19617
          _rtB->apfix_h = 0;

19618
          _rtB->apcheck_b = 0;

19619
          _rtB->fmCruisetoAL = 0;

19620
          _rtB->K_b = 2;

19621
          _rtB->StartEmNav_l = 0;

19622


19623
          /* Entry Internal 'Cruise': '<S325>:38' */

19624
          ((uint8_T *)ssGetDWork(S, 560))[0] = 1U;

19625


19626
          /* Entry Internal 'AltitudeControl': '<S325>:773' */

19627
          /* Transition: '<S325>:808' */

19628
          ((uint8_T *)ssGetDWork(S, 559))[0] = AFCS_MODEL1_IN_AltControl;

19629


19630
          /* Entry Internal 'AltControl': '<S325>:807' */

19631
          /* Transition: '<S325>:814' */

19632
          ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_Hcontrol;

19633


19634
          /* Entry 'Hcontrol': '<S325>:829' */

19635
          _rtB->aplonL_i = 4;

19636
          _rtB->hcmd_e = _rtB->Saturation2_g;

19637
          _rtB->gamacmd_pg = 0.0F;

19638
          ((uint8_T *)ssGetDWork(S, 563))[0] = 1U;

19639


19640
          /* Entry 'PhiControl': '<S325>:780' */

19641
          _rtB->CutPiontFG = 0;

19642
          _rtB->TargetPiont = 0;

19643


19644
          /* Entry Internal 'PhiControl': '<S325>:780' */

19645
          /* Transition: '<S325>:781' */

19646
          ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_PhiControl1;

19647


19648
          /* Entry 'PhiControl1': '<S325>:785' */

19649
          _rtB->aplatL_i = 2;

19650


19651
          /* 滚转角控制模式 */

19652
          _rtB->phiLcmd_n = 0.0F;

19653
          ((uint8_T *)ssGetDWork(S, 565))[0] = 1U;

19654


19655
          /* Entry 'ThrustControl': '<S325>:796' */

19656
          /* Entry Internal 'ThrustControl': '<S325>:796' */

19657
          /* Transition: '<S325>:797' */

19658
          ((uint8_T *)ssGetDWork(S, 564))[0] = AFCS_MODEL1_IN_thrustControl1;

19659


19660
          /* Entry 'thrustControl1': '<S325>:803' */

19661
          _rtB->apthL_k = 3;

19662
          ((uint8_T *)ssGetDWork(S, 566))[0] = 1U;

19663


19664
          /* Entry 'fmphaseTranse': '<S325>:769' */

19665
          ((int32_T *)ssGetDWork(S, 319))[0] = 0;

19666
        } else {

19667
          /* During 'Cruise': '<S325>:38' */

19668
          _rtB->hcmd_e = _rtB->Saturation2_g;

19669
          _rtB->K_b = _rtB->y_p;

19670


19671
          /* During 'AltitudeControl': '<S325>:773' */

19672
          if (((uint8_T *)ssGetDWork(S, 559))[0] == AFCS_MODEL1_IN_AltControl) {

19673
            /* During 'AltControl': '<S325>:807' */

19674
            if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] == 1) {

19675
              /* Transition: '<S325>:823' */

19676
              /* Exit Internal 'AltControl': '<S325>:807' */

19677
              ((uint8_T *)ssGetDWork(S, 561))[0] =

19678
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

19679
              ((uint8_T *)ssGetDWork(S, 559))[0] = AFCS_MODEL1_IN_HdotControl;

19680


19681
              /* Entry 'HdotControl': '<S325>:817' */

19682
              _rtB->aplonL_i = 5;

19683
            } else {

19684
              switch (((uint8_T *)ssGetDWork(S, 561))[0]) {

19685
               case AFCS_MODEL1_IN_Hcontrol:

19686
                /* During 'Hcontrol': '<S325>:829' */

19687
                if (_rtB->Saturation2_g - _rtB->mh_j > 50.0F) {

19688
                  /* Transition: '<S325>:815' */

19689
                  ((uint8_T *)ssGetDWork(S, 561))[0] =

19690
                    AFCS_MODEL1_IN_gamacontrol;

19691


19692
                  /* Entry 'gamacontrol': '<S325>:827' */

19693
                  _rtB->aplonL_i = 3;

19694
                  _rtB->gamacmd_pg = 5.0F;

19695
                } else {

19696
                  if (_rtB->Saturation2_g - _rtB->mh_j < -50.0F) {

19697
                    /* Transition: '<S325>:816' */

19698
                    ((uint8_T *)ssGetDWork(S, 561))[0] =

19699
                      AFCS_MODEL1_IN_gamacontrol1;

19700


19701
                    /* Entry 'gamacontrol1': '<S325>:828' */

19702
                    _rtB->aplonL_i = 3;

19703
                    _rtB->gamacmd_pg = -5.0F;

19704
                  }

19705
                }

19706
                break;

19707


19708
               case AFCS_MODEL1_IN_gamacontrol:

19709
                /* During 'gamacontrol': '<S325>:827' */

19710
                if ((real32_T)fabs(_rtB->Saturation2_g - _rtB->mh_j) < 50.0F) {

19711
                  /* Transition: '<S325>:831' */

19712
                  ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_Hcontrol;

19713


19714
                  /* Entry 'Hcontrol': '<S325>:829' */

19715
                  _rtB->aplonL_i = 4;

19716
                  _rtB->gamacmd_pg = 0.0F;

19717
                }

19718
                break;

19719


19720
               default:

19721
                /* During 'gamacontrol1': '<S325>:828' */

19722
                if ((real32_T)fabs(_rtB->Saturation2_g - _rtB->mh_j) < 50.0F) {

19723
                  /* Transition: '<S325>:830' */

19724
                  ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_Hcontrol;

19725


19726
                  /* Entry 'Hcontrol': '<S325>:829' */

19727
                  _rtB->aplonL_i = 4;

19728
                  _rtB->gamacmd_pg = 0.0F;

19729
                }

19730
                break;

19731
              }

19732
            }

19733
          } else {

19734
            /* During 'HdotControl': '<S325>:817' */

19735
            if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] != 1) {

19736
              /* Transition: '<S325>:824' */

19737
              ((uint8_T *)ssGetDWork(S, 559))[0] = AFCS_MODEL1_IN_AltControl;

19738


19739
              /* Entry Internal 'AltControl': '<S325>:807' */

19740
              /* Transition: '<S325>:814' */

19741
              ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_Hcontrol;

19742


19743
              /* Entry 'Hcontrol': '<S325>:829' */

19744
              _rtB->aplonL_i = 4;

19745
              _rtB->gamacmd_pg = 0.0F;

19746
            } else {

19747
              /* gamacmd=single(0); */

19748
              /* gamacmd=asind(CMD_Hdotc/vt); */

19749
            }

19750
          }

19751


19752
          /* During 'PhiControl': '<S325>:780' */

19753
          switch (((uint8_T *)ssGetDWork(S, 562))[0]) {

19754
           case AFCS_MODEL_IN_GPSFailPhiControl:

19755
            /* During 'GPSFailPhiControl': '<S325>:786' */

19756
            if (_rtB->GPSFail == 0) {

19757
              /* Transition: '<S325>:783' */

19758
              ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_PhiControl1;

19759


19760
              /* Entry 'PhiControl1': '<S325>:785' */

19761
              _rtB->aplatL_i = 2;

19762


19763
              /* 滚转角控制模式 */

19764
              _rtB->phiLcmd_n = 0.0F;

19765
            }

19766
            break;

19767


19768
           case AFCS_MODEL1_IN_PhiControl1:

19769
            /* During 'PhiControl1': '<S325>:785' */

19770
            if (_rtB->GPSFail == 1) {

19771
              /* Transition: '<S325>:782' */

19772
              ((uint8_T *)ssGetDWork(S, 562))[0] =

19773
                AFCS_MODEL_IN_GPSFailPhiControl;

19774


19775
              /* Entry 'GPSFailPhiControl': '<S325>:786' */

19776
              _rtB->aplatL_i = 2;

19777
              _rtB->apyawL_mn = 1;

19778
              _rtB->phiLcmd_n = 30.0F;

19779
            } else if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] ==

19780
                       1) {

19781
              /* Transition: '<S325>:791' */

19782
              ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_YD_MODE;

19783


19784
              /* Entry 'YD_MODE': '<S325>:790' */

19785
              _rtB->aplatL_i = 2;

19786
              _rtB->apyawL_mn = 1;

19787
            } else if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] ==

19788
                       2) {

19789
              /* Transition: '<S325>:860' */

19790
              i_1 = _rtB->CutPiontFG + 1;

19791
              if (i_1 > 127) {

19792
                i_1 = 127;

19793
              }

19794


19795
              _rtB->CutPiontFG = (int8_T)i_1;

19796


19797
              /* 复飞 */

19798
              _rtB->TargetPiont = 2;

19799
              ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_ReFly_MODE;

19800


19801
              /* Entry 'ReFly_MODE': '<S325>:858' */

19802
              _rtB->aplatL_i = 2;

19803


19804
              /* 滚转角控制模式 */

19805
              _rtB->phiLcmd_n = 0.0F;

19806
            } else {

19807
              _rtB->phiLcmd_n = _rtB->Saturation_h;

19808
            }

19809
            break;

19810


19811
           case AFCS_MODEL1_IN_ReFly_MODE:

19812
            /* During 'ReFly_MODE': '<S325>:858' */

19813
            if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] == 1) {

19814
              /* Transition: '<S325>:859' */

19815
              ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_YD_MODE;

19816


19817
              /* Entry 'YD_MODE': '<S325>:790' */

19818
              _rtB->aplatL_i = 2;

19819
              _rtB->apyawL_mn = 1;

19820
            } else if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] ==

19821
                       0) {

19822
              /* Transition: '<S325>:861' */

19823
              ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_PhiControl1;

19824


19825
              /* Entry 'PhiControl1': '<S325>:785' */

19826
              _rtB->aplatL_i = 2;

19827


19828
              /* 滚转角控制模式 */

19829
              _rtB->phiLcmd_n = 0.0F;

19830
            } else {

19831
              _rtB->phiLcmd_n = _rtB->Saturation_h;

19832
            }

19833
            break;

19834


19835
           default:

19836
            /* During 'YD_MODE': '<S325>:790' */

19837
            if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] == 0) {

19838
              /* Transition: '<S325>:847' */

19839
              i_1 = _rtB->CutPiontFG + 1;

19840
              if (i_1 > 127) {

19841
                i_1 = 127;

19842
              }

19843


19844
              _rtB->CutPiontFG = (int8_T)i_1;

19845


19846
              /* 由引导模式切回自主，需要切一次航点 */

19847
              _rtB->TargetPiont = _rtB->y_p;

19848
              ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_PhiControl1;

19849


19850
              /* Entry 'PhiControl1': '<S325>:785' */

19851
              _rtB->aplatL_i = 2;

19852


19853
              /* 滚转角控制模式 */

19854
              _rtB->phiLcmd_n = 0.0F;

19855
            } else if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] ==

19856
                       2) {

19857
              /* Transition: '<S325>:855' */

19858
              i_1 = _rtB->CutPiontFG + 1;

19859
              if (i_1 > 127) {

19860
                i_1 = 127;

19861
              }

19862


19863
              _rtB->CutPiontFG = (int8_T)i_1;

19864


19865
              /* 复飞 */

19866
              _rtB->TargetPiont = 2;

19867
              ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_ReFly_MODE;

19868


19869
              /* Entry 'ReFly_MODE': '<S325>:858' */

19870
              _rtB->aplatL_i = 2;

19871


19872
              /* 滚转角控制模式 */

19873
              _rtB->phiLcmd_n = 0.0F;

19874
            } else {

19875
              _rtB->phiLcmd_n = *((const real32_T **)ssGetInputPortSignalPtrs(S,

19876
                3))[0];

19877
            }

19878
            break;

19879
          }

19880


19881
          /* During 'ThrustControl': '<S325>:796' */

19882
          if (((uint8_T *)ssGetDWork(S, 564))[0] == AFCS_MODEL1_IN_AT_MODE) {

19883
            /* During 'AT_MODE': '<S325>:802' */

19884
            if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 1))[0] == 0) {

19885
              /* Transition: '<S325>:799' */

19886
              ((uint8_T *)ssGetDWork(S, 564))[0] = AFCS_MODEL1_IN_thrustControl1;

19887


19888
              /* Entry 'thrustControl1': '<S325>:803' */

19889
              _rtB->apthL_k = 3;

19890
            } else {

19891
              _rtB->vtcmd_l = *((const real32_T **)ssGetInputPortSignalPtrs(S, 2))

19892
                [0];

19893
            }

19894
          } else {

19895
            /* During 'thrustControl1': '<S325>:803' */

19896
            if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 1))[0] == 1) {

19897
              /* Transition: '<S325>:798' */

19898
              ((uint8_T *)ssGetDWork(S, 564))[0] = AFCS_MODEL1_IN_AT_MODE;

19899


19900
              /* Entry 'AT_MODE': '<S325>:802' */

19901
              _rtB->apthL_k = 3;

19902
            } else {

19903
              _rtB->vtcmd_l = 40.0F;

19904
            }

19905
          }

19906


19907
          /* During 'fmphaseTranse': '<S325>:769' */

19908
          /* %Cruise1FG=(VxdNest==int32(7000)); */

19909
          i_1 = _rtB->Vxd1;

19910
          if (_rtB->Vxd1 > 127) {

19911
            i_1 = 127;

19912
          } else {

19913
            if (_rtB->Vxd1 < -128) {

19914
              i_1 = -128;

19915
            }

19916
          }

19917


19918
          ((int32_T *)ssGetDWork(S, 319))[0] = AFCS_MODEL1_addup_o(_rtB->K_b ==

19919
            i_1, 5, ((int32_T *)ssGetDWork(S, 319))[0]);

19920
          if ((((int32_T *)ssGetDWork(S, 319))[0] == 5) || (_rtB->EnLowFlag == 1)

19921
              || (_rtB->EmlandFg == 1)) {

19922
            /* Transition: '<S325>:770' */

19923
            _rtB->fmCruisetoAL = 1;

19924
          }

19925
        }

19926


19927
        /* End of Chart: '<S315>/Cruise' */

19928
      }

19929


19930
      if (1) {

19931
        /* SignalConversion: '<S315>/Signal Conversion' */

19932
        _rtB->configure = _rtB->configure_j;

19933


19934
        /* SignalConversion: '<S315>/Signal Conversion' */

19935
        _rtB->vtcmd = _rtB->vtcmd_l;

19936


19937
        /* SignalConversion: '<S315>/Signal Conversion' */

19938
        _rtB->nzcmd = _rtB->nzcmd_n;

19939


19940
        /* SignalConversion: '<S315>/Signal Conversion' */

19941
        _rtB->nycmd = _rtB->nycmd_l;

19942


19943
        /* SignalConversion: '<S315>/Signal Conversion' */

19944
        _rtB->phiLcmd = _rtB->phiLcmd_n;

19945


19946
        /* SignalConversion: '<S315>/Signal Conversion' */

19947
        _rtB->thetaLcmd = _rtB->thetaLcmd_a;

19948


19949
        /* SignalConversion: '<S315>/Signal Conversion' */

19950
        _rtB->psiLcmd = _rtB->psiLcmd_e;

19951


19952
        /* SignalConversion: '<S315>/Signal Conversion' */

19953
        _rtB->gamacmd = _rtB->gamacmd_pg;

19954


19955
        /* SignalConversion: '<S315>/Signal Conversion' */

19956
        _rtB->hcmd = _rtB->hcmd_e;

19957


19958
        /* SignalConversion: '<S315>/Signal Conversion' */

19959
        _rtB->psdcmd = _rtB->psdcmd_ik;

19960


19961
        /* SignalConversion: '<S315>/Signal Conversion' */

19962
        _rtB->yrwcmd = _rtB->yrwcmd_f;

19963


19964
        /* SignalConversion: '<S315>/Signal Conversion' */

19965
        _rtB->apthL = _rtB->apthL_k;

19966


19967
        /* SignalConversion: '<S315>/Signal Conversion' */

19968
        _rtB->apcheck = _rtB->apcheck_b;

19969


19970
        /* SignalConversion: '<S315>/Signal Conversion' */

19971
        _rtB->apfix = _rtB->apfix_h;

19972


19973
        /* SignalConversion: '<S315>/Signal Conversion' */

19974
        _rtB->K = _rtB->K_b;

19975


19976
        /* SignalConversion: '<S315>/Signal Conversion' */

19977
        _rtB->StartEmNav = _rtB->StartEmNav_l;

19978


19979
        /* SignalConversion: '<S315>/Signal Conversion' */

19980
        _rtB->aplonL = _rtB->aplonL_i;

19981


19982
        /* SignalConversion: '<S315>/Signal Conversion' */

19983
        _rtB->aplatL = _rtB->aplatL_i;

19984


19985
        /* SignalConversion: '<S315>/Signal Conversion' */

19986
        _rtB->apyawL = _rtB->apyawL_mn;

19987


19988
        /* SignalConversion: '<S315>/Signal Conversion' */

19989
        _rtB->apsbL = _rtB->apsbL_g;

19990


19991
        /* SignalConversion: '<S315>/Signal Conversion' */

19992
        _rtB->apnwsL = _rtB->apnwsL_o;

19993


19994
        /* SignalConversion: '<S315>/Signal Conversion' */

19995
        _rtB->apbrkL = _rtB->apbrkL_a;

19996


19997
        /* SignalConversion: '<S315>/Signal Conversion' */

19998
        _rtB->thrustcmd = _rtB->thrustcmd_f;

19999
      }

20000


20001
      /* End of Outputs for SubSystem: '<S31>/Cruise' */

20002
      break;

20003


20004
     case 4:

20005
      /* Outputs for IfAction SubSystem: '<S31>/Approach' incorporates:

20006
       *  ActionPort: '<S313>/Action Port'

20007
       */

20008
      if (1) {

20009
        /* Sum: '<S313>/Sum' incorporates:

20010
         *  Constant: '<S313>/Constant4'

20011
         */

20012
        _rtB->Sum_ov = (int8_T)(_rtB->NumPoint - 3);

20013
      }

20014


20015
      if (ssIsSampleHit(S, 1, 0)) {

20016
        /* Chart: '<S313>/Approach2' incorporates:

20017
         *  Constant: '<S17>/Constant10'

20018
         *  Constant: '<S17>/Constant15'

20019
         *  Constant: '<S17>/Constant16'

20020
         *  Constant: '<S17>/Constant6'

20021
         */

20022
        /* Gateway: AFCS_MODEL1/GNC/logic/Approach/Approach2 */

20023
        /* During: AFCS_MODEL1/GNC/logic/Approach/Approach2 */

20024
        if (((uint8_T *)ssGetDWork(S, 547))[0] == 0U) {

20025
          /* Entry: AFCS_MODEL1/GNC/logic/Approach/Approach2 */

20026
          ((uint8_T *)ssGetDWork(S, 547))[0] = 1U;

20027


20028
          /* Entry Internal: AFCS_MODEL1/GNC/logic/Approach/Approach2 */

20029
          ((uint8_T *)ssGetDWork(S, 548))[0] = AFCS_MODEL1_IN_Approach;

20030


20031
          /* Entry 'Approach': '<S323>:38' */

20032
          _rtB->configure_c = 1;

20033
          _rtB->apthL_f = 3;

20034
          _rtB->aplonL_o1 = 4;

20035
          _rtB->apsbL_p = 1;

20036
          _rtB->apnwsL_i = 1;

20037
          _rtB->apbrkL_ot = 1;

20038
          _rtB->thrustcmd_e = 0.0F;

20039
          _rtB->vtcmd_h = 40.0F;

20040
          _rtB->nzcmd_b = 0.0F;

20041
          _rtB->nycmd_p = 0.0F;

20042
          _rtB->phiLcmd_l4 = 0.0F;

20043
          _rtB->thetaLcmd_l = 0.0F;

20044
          _rtB->psiLcmd_n = 0.0F;

20045
          _rtB->gamacmd_c = 0.0F;

20046
          _rtB->hcmd_g = _rtB->href;

20047
          _rtB->yrwcmd_i = 0.0F;

20048
          _rtB->apfix_e = 0;

20049
          _rtB->apcheck_h = 0;

20050
          _rtB->fmALtoLand = 0;

20051
          _rtB->K_m = _rtB->Sum_ov;

20052
          _rtB->StartEmNav_o = 0;

20053


20054
          /* Entry Internal 'Approach': '<S323>:38' */

20055
          /* Transition: '<S323>:95' */

20056
          ((uint8_T *)ssGetDWork(S, 549))[0] = AFCS_MODEL1_IN_FlightMode;

20057


20058
          /* Entry 'FlightMode': '<S323>:287' */

20059
          ((int32_T *)ssGetDWork(S, 313))[0] = 0;

20060


20061
          /* Entry Internal 'FlightMode': '<S323>:287' */

20062
          ((uint8_T *)ssGetDWork(S, 550))[0] = 1U;

20063
          ((uint8_T *)ssGetDWork(S, 552))[0] = 1U;

20064


20065
          /* Entry 'XYPositionControl': '<S323>:452' */

20066
          ((int32_T *)ssGetDWork(S, 317))[0] = 0;

20067


20068
          /* Entry Internal 'XYPositionControl': '<S323>:452' */

20069
          /* Transition: '<S323>:487' */

20070
          ((uint8_T *)ssGetDWork(S, 551))[0] = AFCS_MODEL1_IN_NormYrwControl;

20071


20072
          /* Entry 'NormYrwControl': '<S323>:454' */

20073
          /* Entry Internal 'NormYrwControl': '<S323>:454' */

20074
          /* Transition: '<S323>:455' */

20075
          ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_YrwControl1;

20076


20077
          /* Entry Internal 'YrwControl1': '<S323>:550' */

20078
          /* Transition: '<S323>:551' */

20079
          ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_phiLcontrol;

20080


20081
          /* Entry 'phiLcontrol': '<S323>:554' */

20082
          _rtB->aplatL_a = 5;

20083
          _rtB->apyawL_fh = 1;

20084
          _rtB->psdcmd_g = 0.0F;

20085
          ((int32_T *)ssGetDWork(S, 314))[0] = 0;

20086
        } else {

20087
          /* During 'Approach': '<S323>:38' */

20088
          _rtB->hcmd_g = _rtB->href;

20089
          _rtB->vtcmd_h = 40.0F;

20090
          if (((uint8_T *)ssGetDWork(S, 549))[0] == AFCS_MODEL1_IN_FlightMode) {

20091
            /* During 'FlightMode': '<S323>:287' */

20092
            if (((int32_T *)ssGetDWork(S, 313))[0] == 5) {

20093
              /* Transition: '<S323>:43' */

20094
              /* Exit Internal 'FlightMode': '<S323>:287' */

20095
              /* Exit Internal 'XYPositionControl': '<S323>:452' */

20096
              /* Exit Internal 'EmYrwControl': '<S323>:493' */

20097
              ((uint8_T *)ssGetDWork(S, 555))[0] =

20098
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

20099


20100
              /* Exit Internal 'LevelPosition': '<S323>:567' */

20101
              ((uint8_T *)ssGetDWork(S, 556))[0] =

20102
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

20103
              ((uint8_T *)ssGetDWork(S, 551))[0] =

20104
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

20105


20106
              /* Exit Internal 'NormYrwControl': '<S323>:454' */

20107
              ((uint8_T *)ssGetDWork(S, 553))[0] =

20108
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

20109


20110
              /* Exit Internal 'YrwControl1': '<S323>:550' */

20111
              ((uint8_T *)ssGetDWork(S, 554))[0] =

20112
                AFCS_MODEL1_IN_NO_ACTIVE_CHILD;

20113
              ((uint8_T *)ssGetDWork(S, 552))[0] = 0U;

20114
              ((uint8_T *)ssGetDWork(S, 550))[0] = 0U;

20115
              ((uint8_T *)ssGetDWork(S, 549))[0] = AFCS_MODEL1_IN_TransLand;

20116


20117
              /* Entry 'TransLand': '<S323>:42' */

20118
              _rtB->fmALtoLand = 1;

20119
              i_1 = _rtB->Sum_ov + 1;

20120
              if (i_1 > 127) {

20121
                i_1 = 127;

20122
              }

20123


20124
              _rtB->K_m = (int8_T)i_1;

20125
            } else {

20126
              ((int32_T *)ssGetDWork(S, 313))[0] = AFCS_MODEL1_addup_f

20127
                ((_rtB->drange < 400.0F) && ((real32_T)fabs(_rtB->psd) < 90.0F) &&

20128
                 ((real32_T)fabs(_rtB->href - _rtB->mh_j) < 100.0F) &&

20129
                 ((real32_T)fabs(_rtB->yrw - _rtB->yrwcmd_i) < 100.0F) &&

20130
                 (_rtB->SWAUTO == 1) && (_rtB->StartEmNav_o == 0), 5, ((int32_T *)

20131
                  ssGetDWork(S, 313))[0]);

20132


20133
              /* During 'AltitudeControl': '<S323>:469' */

20134
              /* Transition: '<S323>:471' */

20135
              if (_rtB->hcmd_g - _rtB->mh_j < -50.0F) {

20136
                /* Transition: '<S323>:474' */

20137
                _rtB->aplonL_o1 = 3;

20138
                _rtB->gamacmd_c = -5.0F;

20139
              } else if (_rtB->hcmd_g - _rtB->mh_j > 50.0F) {

20140
                /* Transition: '<S323>:473' */

20141
                _rtB->aplonL_o1 = 3;

20142
                _rtB->gamacmd_c = 5.0F;

20143
              } else {

20144
                /* Transition: '<S323>:472' */

20145
                _rtB->aplonL_o1 = 4;

20146
                _rtB->gamacmd_c = 0.0F;

20147
              }

20148


20149
              AFCS_MODEL1_XYPositionControl(S);

20150
            }

20151
          } else {

20152
            /* During 'TransLand': '<S323>:42' */

20153
          }

20154
        }

20155


20156
        /* End of Chart: '<S313>/Approach2' */

20157
      }

20158


20159
      if (1) {

20160
        /* SignalConversion: '<S313>/Signal Conversion' */

20161
        _rtB->configure = _rtB->configure_c;

20162


20163
        /* SignalConversion: '<S313>/Signal Conversion' */

20164
        _rtB->vtcmd = _rtB->vtcmd_h;

20165


20166
        /* SignalConversion: '<S313>/Signal Conversion' */

20167
        _rtB->nzcmd = _rtB->nzcmd_b;

20168


20169
        /* SignalConversion: '<S313>/Signal Conversion' */

20170
        _rtB->nycmd = _rtB->nycmd_p;

20171


20172
        /* SignalConversion: '<S313>/Signal Conversion' */

20173
        _rtB->phiLcmd = _rtB->phiLcmd_l4;

20174


20175
        /* SignalConversion: '<S313>/Signal Conversion' */

20176
        _rtB->thetaLcmd = _rtB->thetaLcmd_l;

20177


20178
        /* SignalConversion: '<S313>/Signal Conversion' */

20179
        _rtB->psiLcmd = _rtB->psiLcmd_n;

20180


20181
        /* SignalConversion: '<S313>/Signal Conversion' */

20182
        _rtB->gamacmd = _rtB->gamacmd_c;

20183


20184
        /* SignalConversion: '<S313>/Signal Conversion' */

20185
        _rtB->hcmd = _rtB->hcmd_g;

20186


20187
        /* SignalConversion: '<S313>/Signal Conversion' */

20188
        _rtB->psdcmd = _rtB->psdcmd_g;

20189


20190
        /* SignalConversion: '<S313>/Signal Conversion' */

20191
        _rtB->yrwcmd = _rtB->yrwcmd_i;

20192


20193
        /* SignalConversion: '<S313>/Signal Conversion' */

20194
        _rtB->apthL = _rtB->apthL_f;

20195


20196
        /* SignalConversion: '<S313>/Signal Conversion' */

20197
        _rtB->apcheck = _rtB->apcheck_h;

20198


20199
        /* SignalConversion: '<S313>/Signal Conversion' */

20200
        _rtB->apfix = _rtB->apfix_e;

20201


20202
        /* SignalConversion: '<S313>/Signal Conversion' */

20203
        _rtB->K = _rtB->K_m;

20204


20205
        /* SignalConversion: '<S313>/Signal Conversion' */

20206
        _rtB->StartEmNav = _rtB->StartEmNav_o;

20207


20208
        /* SignalConversion: '<S313>/Signal Conversion' */

20209
        _rtB->aplonL = _rtB->aplonL_o1;

20210


20211
        /* SignalConversion: '<S313>/Signal Conversion' */

20212
        _rtB->aplatL = _rtB->aplatL_a;

20213


20214
        /* SignalConversion: '<S313>/Signal Conversion' */

20215
        _rtB->apyawL = _rtB->apyawL_fh;

20216


20217
        /* SignalConversion: '<S313>/Signal Conversion' */

20218
        _rtB->apsbL = _rtB->apsbL_p;

20219


20220
        /* SignalConversion: '<S313>/Signal Conversion' */

20221
        _rtB->apnwsL = _rtB->apnwsL_i;

20222


20223
        /* SignalConversion: '<S313>/Signal Conversion' */

20224
        _rtB->apbrkL = _rtB->apbrkL_ot;

20225


20226
        /* SignalConversion: '<S313>/Signal Conversion' */

20227
        _rtB->thrustcmd = _rtB->thrustcmd_e;

20228
      }

20229


20230
      /* End of Outputs for SubSystem: '<S31>/Approach' */

20231
      break;

20232


20233
     case 5:

20234
      /* Outputs for IfAction SubSystem: '<S31>/Landing' incorporates:

20235
       *  ActionPort: '<S316>/Action Port'

20236
       */

20237
      if (1) {

20238
        /* Sum: '<S316>/Sum' incorporates:

20239
         *  Constant: '<S316>/Constant4'

20240
         */

20241
        _rtB->Sum_mn = (int8_T)(_rtB->NumPoint - 2);

20242
      }

20243


20244
      if (ssIsSampleHit(S, 1, 0)) {

20245
        /* Chart: '<S316>/Landing2' incorporates:

20246
         *  Constant: '<S17>/Constant6'

20247
         */

20248
        /* Gateway: AFCS_MODEL1/GNC/logic/Landing/Landing2 */

20249
        /* During: AFCS_MODEL1/GNC/logic/Landing/Landing2 */

20250
        if (((uint8_T *)ssGetDWork(S, 532))[0] == 0U) {

20251
          /* Entry: AFCS_MODEL1/GNC/logic/Landing/Landing2 */

20252
          ((uint8_T *)ssGetDWork(S, 532))[0] = 1U;

20253


20254
          /* Entry Internal: AFCS_MODEL1/GNC/logic/Landing/Landing2 */

20255
          ((uint8_T *)ssGetDWork(S, 533))[0] = AFCS_MODEL1_IN_Landing;

20256


20257
          /* Entry 'Landing': '<S326>:38' */

20258
          _rtB->configure_gv = 1;

20259
          _rtB->apthL_e = 3;

20260
          _rtB->aplonL_f = 4;

20261
          _rtB->apyawL_m = 1;

20262
          _rtB->apsbL_l = 1;

20263
          _rtB->apnwsL_m = 1;

20264
          _rtB->apbrkL_n = 1;

20265
          _rtB->thrustcmd_ho = 0.0F;

20266
          _rtB->vtcmd_f = 40.0F;

20267
          _rtB->nzcmd_i = 0.0F;

20268
          _rtB->nycmd_a = 0.0F;

20269
          _rtB->phiLcmd_c = 0.0F;

20270
          _rtB->thetaLcmd_d = 0.0F;

20271
          _rtB->psiLcmd_i = 0.0F;

20272
          _rtB->gamacmd_k = 0.0F;

20273
          _rtB->hcmd_j = _rtB->HrefAL;

20274
          _rtB->yrwcmd_o = 0.0F;

20275
          _rtB->apfix_p = 0;

20276
          _rtB->apcheck_p = 0;

20277
          _rtB->fmLandtoTaxi = 0;

20278
          _rtB->K_k = _rtB->Sum_mn;

20279
          _rtB->StartEmNav_a = 0;

20280
          ((int32_T *)ssGetDWork(S, 308))[0] = 0;

20281


20282
          /* Entry Internal 'Landing': '<S326>:38' */

20283
          /* Transition: '<S326>:347' */

20284
          ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Pre_Land;

20285


20286
          /* Entry 'Pre_Land': '<S326>:357' */

20287
          ((int32_T *)ssGetDWork(S, 301))[0] = 0;

20288


20289
          /* Entry Internal 'Pre_Land': '<S326>:357' */

20290
          ((uint8_T *)ssGetDWork(S, 535))[0] = 1U;

20291
          ((uint8_T *)ssGetDWork(S, 537))[0] = 1U;

20292


20293
          /* Entry Internal 'XYPositionControl': '<S326>:367' */

20294
          /* Transition: '<S326>:368' */

20295
          ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;

20296


20297
          /* Entry Internal 'YrwControl1': '<S326>:369' */

20298
          /* Transition: '<S326>:370' */

20299
          ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;

20300


20301
          /* Entry 'phiLcontrol': '<S326>:372' */

20302
          _rtB->aplatL_l = 5;

20303
          _rtB->psdcmd_i = 0.0F;

20304
          ((int32_T *)ssGetDWork(S, 305))[0] = 0;

20305
        } else {

20306
          AFCS_MODEL1_Landing(S);

20307
        }

20308


20309
        /* End of Chart: '<S316>/Landing2' */

20310
      }

20311


20312
      if (1) {

20313
        /* SignalConversion: '<S316>/Signal Conversion' */

20314
        _rtB->configure = _rtB->configure_gv;

20315


20316
        /* SignalConversion: '<S316>/Signal Conversion' */

20317
        _rtB->vtcmd = _rtB->vtcmd_f;

20318


20319
        /* SignalConversion: '<S316>/Signal Conversion' */

20320
        _rtB->nzcmd = _rtB->nzcmd_i;

20321


20322
        /* SignalConversion: '<S316>/Signal Conversion' */

20323
        _rtB->nycmd = _rtB->nycmd_a;

20324


20325
        /* SignalConversion: '<S316>/Signal Conversion' */

20326
        _rtB->phiLcmd = _rtB->phiLcmd_c;

20327


20328
        /* SignalConversion: '<S316>/Signal Conversion' */

20329
        _rtB->thetaLcmd = _rtB->thetaLcmd_d;

20330


20331
        /* SignalConversion: '<S316>/Signal Conversion' */

20332
        _rtB->psiLcmd = _rtB->psiLcmd_i;

20333


20334
        /* SignalConversion: '<S316>/Signal Conversion' */

20335
        _rtB->gamacmd = _rtB->gamacmd_k;

20336


20337
        /* SignalConversion: '<S316>/Signal Conversion' */

20338
        _rtB->hcmd = _rtB->hcmd_j;

20339


20340
        /* SignalConversion: '<S316>/Signal Conversion' */

20341
        _rtB->psdcmd = _rtB->psdcmd_i;

20342


20343
        /* SignalConversion: '<S316>/Signal Conversion' */

20344
        _rtB->yrwcmd = _rtB->yrwcmd_o;

20345


20346
        /* SignalConversion: '<S316>/Signal Conversion' */

20347
        _rtB->apthL = _rtB->apthL_e;

20348


20349
        /* SignalConversion: '<S316>/Signal Conversion' */

20350
        _rtB->apcheck = _rtB->apcheck_p;

20351


20352
        /* SignalConversion: '<S316>/Signal Conversion' */

20353
        _rtB->apfix = _rtB->apfix_p;

20354


20355
        /* SignalConversion: '<S316>/Signal Conversion' */

20356
        _rtB->K = _rtB->K_k;

20357


20358
        /* SignalConversion: '<S316>/Signal Conversion' */

20359
        _rtB->StartEmNav = _rtB->StartEmNav_a;

20360


20361
        /* SignalConversion: '<S316>/Signal Conversion' */

20362
        _rtB->aplonL = _rtB->aplonL_f;

20363


20364
        /* SignalConversion: '<S316>/Signal Conversion' */

20365
        _rtB->aplatL = _rtB->aplatL_l;

20366


20367
        /* SignalConversion: '<S316>/Signal Conversion' */

20368
        _rtB->apyawL = _rtB->apyawL_m;

20369


20370
        /* SignalConversion: '<S316>/Signal Conversion' */

20371
        _rtB->apsbL = _rtB->apsbL_l;

20372


20373
        /* SignalConversion: '<S316>/Signal Conversion' */

20374
        _rtB->apnwsL = _rtB->apnwsL_m;

20375


20376
        /* SignalConversion: '<S316>/Signal Conversion' */

20377
        _rtB->apbrkL = _rtB->apbrkL_n;

20378


20379
        /* SignalConversion: '<S316>/Signal Conversion' */

20380
        _rtB->thrustcmd = _rtB->thrustcmd_ho;

20381
      }

20382


20383
      /* End of Outputs for SubSystem: '<S31>/Landing' */

20384
      break;

20385


20386
     case 6:

20387
      /* Outputs for IfAction SubSystem: '<S31>/Taxi' incorporates:

20388
       *  ActionPort: '<S322>/Action Port'

20389
       */

20390
      if (ssIsSampleHit(S, 1, 0)) {

20391
        /* Chart: '<S322>/Taxi' incorporates:

20392
         *  Constant: '<S17>/Constant7'

20393
         *  Constant: '<S17>/Constant8'

20394
         */

20395
        /* Gateway: AFCS_MODEL1/GNC/logic/Taxi/Taxi */

20396
        /* During: AFCS_MODEL1/GNC/logic/Taxi/Taxi */

20397
        if (((uint8_T *)ssGetDWork(S, 527))[0] == 0U) {

20398
          /* Entry: AFCS_MODEL1/GNC/logic/Taxi/Taxi */

20399
          ((uint8_T *)ssGetDWork(S, 527))[0] = 1U;

20400


20401
          /* Entry Internal: AFCS_MODEL1/GNC/logic/Taxi/Taxi */

20402
          ((uint8_T *)ssGetDWork(S, 528))[0] = AFCS_MODEL1_IN_Taxi;

20403


20404
          /* Entry 'Taxi': '<S330>:38' */

20405
          _rtB->configure_g = 1;

20406
          _rtB->apthL_i = 1;

20407
          _rtB->aplonL_o = 2;

20408
          _rtB->aplatL_f = 2;

20409
          _rtB->apyawL_f = 3;

20410
          _rtB->apsbL_e = 2;

20411


20412
          /* 着陆滑跑,阻力舵0.5 */

20413
          _rtB->apbrkL_o = 1;

20414
          _rtB->thrustcmd_h = 0.0F;

20415
          _rtB->vtcmd_k = 0.0F;

20416
          _rtB->nzcmd_l = 0.0F;

20417
          _rtB->nycmd_m = 0.0F;

20418
          _rtB->phiLcmd_l = 0.0F;

20419
          _rtB->thetaLcmd_f = 2.0F;

20420
          _rtB->psiLcmd_c = 0.0F;

20421
          _rtB->gamacmd_p = 0.0F;

20422
          _rtB->hcmd_k = 0.0F;

20423
          _rtB->psdcmd_c = 0.0F;

20424
          _rtB->yrwcmd_k = 0.0F;

20425
          _rtB->apfix_c = 0;

20426
          _rtB->apcheck_f = 0;

20427
          _rtB->K_c = _rtB->UnitDelay;

20428
          _rtB->StartEmNav_b = 0;

20429


20430
          /* Entry Internal 'Taxi': '<S330>:38' */

20431
          ((uint8_T *)ssGetDWork(S, 530))[0] = 1U;

20432


20433
          /* Entry Internal 'TaxiControl': '<S330>:190' */

20434
          /* Transition: '<S330>:198' */

20435
          ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_DrControl;

20436


20437
          /* Entry 'DrControl': '<S330>:194' */

20438
          _rtB->apnwsL_k = 1;

20439
          ((int32_T *)ssGetDWork(S, 297))[0] = 0;

20440
          ((uint8_T *)ssGetDWork(S, 531))[0] = 1U;

20441


20442
          /* Entry Internal 'PointControl': '<S330>:199' */

20443
          /* Transition: '<S330>:202' */

20444
          if (_rtB->AbortTakeoffFg == 1) {

20445
            /* Transition: '<S330>:204' */

20446
            _rtB->K_c = 1;

20447
          } else {

20448
            /* Transition: '<S330>:206' */

20449
            i_1 = _rtB->IndexAL + 1;

20450
            if (i_1 > 127) {

20451
              i_1 = 127;

20452
            }

20453


20454
            _rtB->K_c = (int8_T)i_1;

20455
          }

20456
        } else {

20457
          /* During 'Taxi': '<S330>:38' */

20458
          /* During 'TaxiControl': '<S330>:190' */

20459
          switch (((uint8_T *)ssGetDWork(S, 529))[0]) {

20460
           case AFCS_MODEL1_IN_DrControl:

20461
            /* During 'DrControl': '<S330>:194' */

20462
            if (((int32_T *)ssGetDWork(S, 297))[0] == 5) {

20463
              /* Transition: '<S330>:191' */

20464
              ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_NwsControl;

20465


20466
              /* Entry 'NwsControl': '<S330>:196' */

20467
              _rtB->apnwsL_k = 2;

20468
              _rtB->apbrkL_o = 2;

20469
              ((int32_T *)ssGetDWork(S, 299))[0] = 0;

20470
            } else {

20471
              ((int32_T *)ssGetDWork(S, 297))[0] = AFCS_MODEL1_addup_od

20472
                ((_rtB->Switch7_n == 1) && (_rtB->Switch6_j == 1), 5, ((int32_T *)

20473
                  ssGetDWork(S, 297))[0]);

20474
            }

20475
            break;

20476


20477
           case AFCS_MODEL1_IN_EndControl:

20478
            /* During 'EndControl': '<S330>:195' */

20479
            break;

20480


20481
           case AFCS_MODEL1_IN_NoDrControl:

20482
            /* During 'NoDrControl': '<S330>:197' */

20483
            if (((int32_T *)ssGetDWork(S, 298))[0] == 5) {

20484
              /* Transition: '<S330>:193' */

20485
              ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_EndControl;

20486


20487
              /* Entry 'EndControl': '<S330>:195' */

20488
              _rtB->apthL_i = 1;

20489
              _rtB->aplonL_o = 1;

20490
              _rtB->aplatL_f = 1;

20491
              _rtB->apyawL_f = 1;

20492
              _rtB->apnwsL_k = 1;

20493
              _rtB->apbrkL_o = 0;

20494
              _rtB->apsbL_e = 1;

20495
            } else {

20496
              ((int32_T *)ssGetDWork(S, 298))[0] = AFCS_MODEL1_addup_od(_rtB->vt

20497
                < 0.5F, 5, ((int32_T *)ssGetDWork(S, 298))[0]);

20498
            }

20499
            break;

20500


20501
           case AFCS_MODEL1_IN_NwsControl:

20502
            /* During 'NwsControl': '<S330>:196' */

20503
            if (((int32_T *)ssGetDWork(S, 299))[0] == 5) {

20504
              /* Transition: '<S330>:192' */

20505
              ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_ThetaControl;

20506


20507
              /* Entry 'ThetaControl': '<S330>:209' */

20508
              _rtB->aplonL_o = 2;

20509
              _rtB->thetaLcmd_f = -5.0F;

20510
              ((int32_T *)ssGetDWork(S, 300))[0] = 0;

20511
            } else {

20512
              ((int32_T *)ssGetDWork(S, 299))[0] = AFCS_MODEL1_addup_od

20513
                (_rtB->Vc_p < 15.0F, 5, ((int32_T *)ssGetDWork(S, 299))[0]);

20514
            }

20515
            break;

20516


20517
           default:

20518
            /* During 'ThetaControl': '<S330>:209' */

20519
            if (((int32_T *)ssGetDWork(S, 300))[0] == 5) {

20520
              /* Transition: '<S330>:211' */

20521
              ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_NoDrControl;

20522


20523
              /* Entry 'NoDrControl': '<S330>:197' */

20524
              _rtB->apyawL_f = 1;

20525
              ((int32_T *)ssGetDWork(S, 298))[0] = 0;

20526
            } else {

20527
              ((int32_T *)ssGetDWork(S, 300))[0] = AFCS_MODEL1_addup_od

20528
                (_rtB->Vc_p < 5.0F, 5, ((int32_T *)ssGetDWork(S, 300))[0]);

20529
            }

20530
            break;

20531
          }

20532


20533
          /* During 'PointControl': '<S330>:199' */

20534
        }

20535


20536
        /* End of Chart: '<S322>/Taxi' */

20537
      }

20538


20539
      if (1) {

20540
        /* SignalConversion: '<S322>/Signal Conversion' */

20541
        _rtB->configure = _rtB->configure_g;

20542


20543
        /* SignalConversion: '<S322>/Signal Conversion' */

20544
        _rtB->vtcmd = _rtB->vtcmd_k;

20545


20546
        /* SignalConversion: '<S322>/Signal Conversion' */

20547
        _rtB->nzcmd = _rtB->nzcmd_l;

20548


20549
        /* SignalConversion: '<S322>/Signal Conversion' */

20550
        _rtB->nycmd = _rtB->nycmd_m;

20551


20552
        /* SignalConversion: '<S322>/Signal Conversion' */

20553
        _rtB->phiLcmd = _rtB->phiLcmd_l;

20554


20555
        /* SignalConversion: '<S322>/Signal Conversion' */

20556
        _rtB->thetaLcmd = _rtB->thetaLcmd_f;

20557


20558
        /* SignalConversion: '<S322>/Signal Conversion' */

20559
        _rtB->psiLcmd = _rtB->psiLcmd_c;

20560


20561
        /* SignalConversion: '<S322>/Signal Conversion' */

20562
        _rtB->gamacmd = _rtB->gamacmd_p;

20563


20564
        /* SignalConversion: '<S322>/Signal Conversion' */

20565
        _rtB->hcmd = _rtB->hcmd_k;

20566


20567
        /* SignalConversion: '<S322>/Signal Conversion' */

20568
        _rtB->psdcmd = _rtB->psdcmd_c;

20569


20570
        /* SignalConversion: '<S322>/Signal Conversion' */

20571
        _rtB->yrwcmd = _rtB->yrwcmd_k;

20572


20573
        /* SignalConversion: '<S322>/Signal Conversion' */

20574
        _rtB->apthL = _rtB->apthL_i;

20575


20576
        /* SignalConversion: '<S322>/Signal Conversion' */

20577
        _rtB->apcheck = _rtB->apcheck_f;

20578


20579
        /* SignalConversion: '<S322>/Signal Conversion' */

20580
        _rtB->apfix = _rtB->apfix_c;

20581


20582
        /* SignalConversion: '<S322>/Signal Conversion' */

20583
        _rtB->K = _rtB->K_c;

20584


20585
        /* SignalConversion: '<S322>/Signal Conversion' */

20586
        _rtB->StartEmNav = _rtB->StartEmNav_b;

20587


20588
        /* SignalConversion: '<S322>/Signal Conversion' */

20589
        _rtB->aplonL = _rtB->aplonL_o;

20590


20591
        /* SignalConversion: '<S322>/Signal Conversion' */

20592
        _rtB->aplatL = _rtB->aplatL_f;

20593


20594
        /* SignalConversion: '<S322>/Signal Conversion' */

20595
        _rtB->apyawL = _rtB->apyawL_f;

20596


20597
        /* SignalConversion: '<S322>/Signal Conversion' */

20598
        _rtB->apsbL = _rtB->apsbL_e;

20599


20600
        /* SignalConversion: '<S322>/Signal Conversion' */

20601
        _rtB->apnwsL = _rtB->apnwsL_k;

20602


20603
        /* SignalConversion: '<S322>/Signal Conversion' */

20604
        _rtB->apbrkL = _rtB->apbrkL_o;

20605


20606
        /* SignalConversion: '<S322>/Signal Conversion' */

20607
        _rtB->thrustcmd = _rtB->thrustcmd_h;

20608
      }

20609


20610
      /* End of Outputs for SubSystem: '<S31>/Taxi' */

20611
      break;

20612
    }

20613


20614
    /* End of SwitchCase: '<S31>/Switch Case' */

20615
  }

20616


20617
  /* Outputs for Atomic SubSystem: '<S7>/Level' */

20618
  if (ssIsSampleHit(S, 1, 0)) {

20619
    /* SignalConversion: '<S22>/TmpSignal ConversionAtCodeReuseSubsystemInport3' */

20620
    _rtB->TmpSignalConversionAtCodeReuseS = _rtB->Saturation1_g;

20621


20622
    /* SignalConversion: '<S22>/TmpSignal ConversionAtCodeReuseSubsystemInport10' incorporates:

20623
     *  Constant: '<S27>/Constant1'

20624
     *  Constant: '<S27>/Constant3'

20625
     *  Constant: '<S27>/Constant5'

20626
     *  Constant: '<S27>/Constant6'

20627
     *  Constant: '<S27>/Constant7'

20628
     *  Constant: '<S27>/Constant8'

20629
     */

20630
    _rtB->TmpSignalConversionAtCodeReus_b[0] = 0.0F;

20631
    _rtB->TmpSignalConversionAtCodeReus_b[1] = 0.0F;

20632
    _rtB->TmpSignalConversionAtCodeReus_b[2] = 0.0F;

20633
    _rtB->TmpSignalConversionAtCodeReus_b[3] = 0.0F;

20634
    _rtB->TmpSignalConversionAtCodeReus_b[4] = 0.0F;

20635
    _rtB->TmpSignalConversionAtCodeReus_b[5] = 0.0F;

20636
  }

20637


20638
  if (1) {

20639
    /* SignalConversion: '<S22>/TmpSignal ConversionAtCodeReuseSubsystemInport12' */

20640
    _rtB->TmpSignalConversionAtCodeReus_i[0] = _rtB->decl_qgc;

20641
    _rtB->TmpSignalConversionAtCodeReus_i[1] = _rtB->dacl_qgc;

20642
    _rtB->TmpSignalConversionAtCodeReus_i[2] = _rtB->drcl_qgc;

20643
    _rtB->TmpSignalConversionAtCodeReus_i[3] = _rtB->nwscl_qgc;

20644
    _rtB->TmpSignalConversionAtCodeReus_i[4] = _rtB->sbcl_qgc;

20645
  }

20646


20647
  /* Outputs for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */

20648
  AFCS_MODEL1_CodeReuseSubsystem(S);

20649


20650
  /* End of Outputs for SubSystem: '<S22>/CodeReuseSubsystem' */

20651


20652
  /* End of Outputs for SubSystem: '<S7>/Level' */

20653
  if (1) {

20654
    /* Outport: '<Root>/De_L' */

20655
    ((real32_T *)ssGetOutputPortSignal(S, 24))[0] = _rtB->decl;

20656


20657
    /* Outport: '<Root>/De_R' */

20658
    ((real32_T *)ssGetOutputPortSignal(S, 25))[0] = _rtB->decl;

20659


20660
    /* Gain: '<S1>/Gain' */

20661
    _rtB->Gain_e = -_rtB->dacl;

20662


20663
    /* Outport: '<Root>/Da_L' */

20664
    ((real32_T *)ssGetOutputPortSignal(S, 26))[0] = _rtB->Gain_e;

20665


20666
    /* Outport: '<Root>/Da_R' */

20667
    ((real32_T *)ssGetOutputPortSignal(S, 27))[0] = _rtB->dacl;

20668


20669
    /* Outport: '<Root>/Dr' */

20670
    ((real32_T *)ssGetOutputPortSignal(S, 28))[0] = _rtB->drcl;

20671


20672
    /* Outport: '<Root>/VIAS' */

20673
    ((real32_T *)ssGetOutputPortSignal(S, 29))[0] = _rtB->Va_l;

20674


20675
    /* Outport: '<Root>/VTAS' */

20676
    ((real32_T *)ssGetOutputPortSignal(S, 30))[0] = _rtB->Vc_p;

20677


20678
    /* Outport: '<Root>/VG' */

20679
    ((real32_T *)ssGetOutputPortSignal(S, 31))[0] = _rtB->vt;

20680
  }

20681


20682
  if (ssIsSampleHit(S, 1, 0)) {

20683
    /* Outport: '<Root>/Flyphase' */

20684
    ((int8_T *)ssGetOutputPortSignal(S, 32))[0] = _rtB->fmphase;

20685
  }

20686


20687
  if (1) {

20688
    /* Switch: '<S23>/Switch1' incorporates:

20689
     *  Constant: '<S23>/Constant'

20690
     */

20691
    if (_rtB->LockFlag > 0) {

20692
      _rtB->thrust = 0.0F;

20693
    } else {

20694
      _rtB->thrust = _rtB->GeneratedSFunction_o1;

20695
    }

20696


20697
    /* End of Switch: '<S23>/Switch1' */

20698
  }

20699


20700
  if (ssIsSampleHit(S, 1, 0)) {

20701
  }

20702


20703
  if (1) {

20704
    /* Switch: '<S7>/Switch' incorporates:

20705
     *  Constant: '<S25>/Constant1'

20706
     *  Constant: '<S25>/Constant10'

20707
     *  Constant: '<S25>/Constant2'

20708
     *  Constant: '<S25>/Constant3'

20709
     *  Constant: '<S25>/Constant4'

20710
     *  Constant: '<S25>/Constant5'

20711
     *  Constant: '<S25>/Constant6'

20712
     *  Constant: '<S25>/Constant7'

20713
     *  Constant: '<S25>/Constant8'

20714
     *  Constant: '<S25>/Constant9'

20715
     */

20716
    if (_rtB->LockFlag > 0) {

20717
      _rtB->Switch_nw[0] = 1000U;

20718
      _rtB->Switch_nw[1] = 1500U;

20719
      _rtB->Switch_nw[2] = 1500U;

20720
      _rtB->Switch_nw[3] = 1500U;

20721
      _rtB->Switch_nw[4] = 1500U;

20722
      _rtB->Switch_nw[5] = 1500U;

20723
      _rtB->Switch_nw[6] = 1000U;

20724
      _rtB->Switch_nw[7] = 1500U;

20725
      _rtB->Switch_nw[8] = 1000U;

20726
      _rtB->Switch_nw[9] = 1000U;

20727
    } else {

20728
      _rtB->Switch_nw[0] = _rtB->Saturation_h4[0];

20729
      _rtB->Switch_nw[1] = _rtB->Saturation_h4[1];

20730
      _rtB->Switch_nw[2] = _rtB->Saturation_h4[2];

20731
      _rtB->Switch_nw[3] = _rtB->Saturation_h4[3];

20732
      _rtB->Switch_nw[4] = _rtB->Saturation_h4[4];

20733
      _rtB->Switch_nw[5] = _rtB->Saturation_h4[5];

20734
      _rtB->Switch_nw[6] = _rtB->Saturation_h4[6];

20735
      _rtB->Switch_nw[7] = _rtB->Saturation_h4[7];

20736
      _rtB->Switch_nw[8] = _rtB->Saturation_h4[8];

20737
      _rtB->Switch_nw[9] = _rtB->Saturation_h4[9];

20738
    }

20739


20740
    /* End of Switch: '<S7>/Switch' */

20741
  }

20742


20743
  if (ssIsSampleHit(S, 1, 0)) {

20744
  }

20745


20746
  if (1) {

20747
    /* Switch: '<S24>/Switch1' incorporates:

20748
     *  Constant: '<S24>/Constant5'

20749
     *  Saturate: '<S24>/Saturation'

20750
     */

20751
    if (_rtB->LogicalOperator1_b) {

20752
      _rtB->UnaryMinus = 0.0;

20753
    } else {

20754
      if (_rtB->vt > 200.0F) {

20755
        /* Saturate: '<S24>/Saturation' */

20756
        _rtB->TrigonometricFunction = 200.0F;

20757
      } else if (_rtB->vt < 0.01F) {

20758
        /* Saturate: '<S24>/Saturation' */

20759
        _rtB->TrigonometricFunction = 0.01F;

20760
      } else {

20761
        /* Saturate: '<S24>/Saturation' */

20762
        _rtB->TrigonometricFunction = _rtB->vt;

20763
      }

20764


20765
      /* Product: '<S24>/Divide' incorporates:

20766
       *  Saturate: '<S24>/Saturation'

20767
       */

20768
      _rtB->TrigonometricFunction = _rtB->mhdot / _rtB->TrigonometricFunction;

20769


20770
      /* Trigonometry: '<S24>/Trigonometric Function' */

20771
      if (_rtB->TrigonometricFunction > 1.0F) {

20772
        DXp = 1.0F;

20773
      } else if (_rtB->TrigonometricFunction < -1.0F) {

20774
        DXp = -1.0F;

20775
      } else {

20776
        DXp = _rtB->TrigonometricFunction;

20777
      }

20778


20779
      _rtB->TrigonometricFunction = (real32_T)asin(DXp);

20780


20781
      /* End of Trigonometry: '<S24>/Trigonometric Function' */

20782


20783
      /* Gain: '<S291>/Unit Conversion' */

20784
      _rtB->UnitConversion_f = 57.2957802F * _rtB->TrigonometricFunction;

20785
      _rtB->UnaryMinus = _rtB->UnitConversion_f;

20786
    }

20787


20788
    /* End of Switch: '<S24>/Switch1' */

20789


20790
    /* DataTypeConversion: '<S24>/Data Type Conversion7' */

20791
    _rtB->gama = (real32_T)_rtB->UnaryMinus;

20792
  }

20793


20794
  if (ssIsSampleHit(S, 1, 0)) {

20795
    /* Chart: '<S21>/delayblock1' */

20796
    /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock1 */

20797
    /* During: AFCS_MODEL1/GNC/Calibration/delayblock1 */

20798
    if (((uint8_T *)ssGetDWork(S, 609))[0] == 0U) {

20799
      /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock1 */

20800
      ((uint8_T *)ssGetDWork(S, 609))[0] = 1U;

20801


20802
      /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock1 */

20803
      ((uint8_T *)ssGetDWork(S, 610))[0] = AFCS_MODEL1_IN_delayblock;

20804


20805
      /* Entry 'delayblock': '<S48>:38' */

20806
      /* Entry Internal 'delayblock': '<S48>:38' */

20807
      /* Transition: '<S48>:164' */

20808
      if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 0) {

20809
        /* Transition: '<S48>:160' */

20810
        ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input0;

20811


20812
        /* Entry 'Input0': '<S48>:138' */

20813
        ((int32_T *)ssGetDWork(S, 342))[0] = 0;

20814
      } else {

20815
        /* Transition: '<S48>:166' */

20816
        ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input1;

20817


20818
        /* Entry 'Input1': '<S48>:165' */

20819
        ((int32_T *)ssGetDWork(S, 342))[0] = 0;

20820
      }

20821
    } else {

20822
      /* During 'delayblock': '<S48>:38' */

20823
      switch (((uint8_T *)ssGetDWork(S, 611))[0]) {

20824
       case AFCS_MODEL1_IN_Input0:

20825
        /* During 'Input0': '<S48>:138' */

20826
        if (((int32_T *)ssGetDWork(S, 342))[0] == 20) {

20827
          /* Transition: '<S48>:137' */

20828
          ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Output0;

20829


20830
          /* Entry 'Output0': '<S48>:130' */

20831
          ((int32_T *)ssGetDWork(S, 342))[0] = 0;

20832
        } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 1) {

20833
          /* Transition: '<S48>:161' */

20834
          ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input1;

20835


20836
          /* Entry 'Input1': '<S48>:165' */

20837
          ((int32_T *)ssGetDWork(S, 342))[0] = 0;

20838
        } else {

20839
          ((int32_T *)ssGetDWork(S, 342))[0] = AFCS_MODEL1_addup(1, 20,

20840
            ((int32_T *)ssGetDWork(S, 342))[0]);

20841
        }

20842
        break;

20843


20844
       case AFCS_MODEL1_IN_Input1:

20845
        /* During 'Input1': '<S48>:165' */

20846
        if (((int32_T *)ssGetDWork(S, 342))[0] == 20) {

20847
          /* Transition: '<S48>:168' */

20848
          ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Output1;

20849


20850
          /* Entry 'Output1': '<S48>:167' */

20851
          ((int32_T *)ssGetDWork(S, 342))[0] = 0;

20852
        } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 0) {

20853
          /* Transition: '<S48>:162' */

20854
          ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input0;

20855


20856
          /* Entry 'Input0': '<S48>:138' */

20857
          ((int32_T *)ssGetDWork(S, 342))[0] = 0;

20858
        } else {

20859
          ((int32_T *)ssGetDWork(S, 342))[0] = AFCS_MODEL1_addup(1, 20,

20860
            ((int32_T *)ssGetDWork(S, 342))[0]);

20861
        }

20862
        break;

20863


20864
       case AFCS_MODEL1_IN_Output0:

20865
        /* During 'Output0': '<S48>:130' */

20866
        if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 1) {

20867
          /* Transition: '<S48>:139' */

20868
          ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input1;

20869


20870
          /* Entry 'Input1': '<S48>:165' */

20871
          ((int32_T *)ssGetDWork(S, 342))[0] = 0;

20872
        }

20873
        break;

20874


20875
       default:

20876
        /* During 'Output1': '<S48>:167' */

20877
        if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 0) {

20878
          /* Transition: '<S48>:170' */

20879
          ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input0;

20880


20881
          /* Entry 'Input0': '<S48>:138' */

20882
          ((int32_T *)ssGetDWork(S, 342))[0] = 0;

20883
        }

20884
        break;

20885
      }

20886
    }

20887


20888
    /* End of Chart: '<S21>/delayblock1' */

20889


20890
    /* Chart: '<S21>/delayblock6' */

20891
    /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock6 */

20892
    /* During: AFCS_MODEL1/GNC/Calibration/delayblock6 */

20893
    if (((uint8_T *)ssGetDWork(S, 594))[0] == 0U) {

20894
      /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock6 */

20895
      ((uint8_T *)ssGetDWork(S, 594))[0] = 1U;

20896


20897
      /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock6 */

20898
      ((uint8_T *)ssGetDWork(S, 595))[0] = AFCS_MODEL1_IN_delayblock;

20899


20900
      /* Entry 'delayblock': '<S53>:38' */

20901
      /* Entry Internal 'delayblock': '<S53>:38' */

20902
      /* Transition: '<S53>:164' */

20903
      if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 0) {

20904
        /* Transition: '<S53>:160' */

20905
        ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input0;

20906


20907
        /* Entry 'Input0': '<S53>:138' */

20908
        ((int32_T *)ssGetDWork(S, 337))[0] = 0;

20909
      } else {

20910
        /* Transition: '<S53>:166' */

20911
        ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input1;

20912


20913
        /* Entry 'Input1': '<S53>:165' */

20914
        ((int32_T *)ssGetDWork(S, 337))[0] = 0;

20915
      }

20916
    } else {

20917
      /* During 'delayblock': '<S53>:38' */

20918
      switch (((uint8_T *)ssGetDWork(S, 596))[0]) {

20919
       case AFCS_MODEL1_IN_Input0:

20920
        /* During 'Input0': '<S53>:138' */

20921
        if (((int32_T *)ssGetDWork(S, 337))[0] == 20) {

20922
          /* Transition: '<S53>:137' */

20923
          ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Output0;

20924


20925
          /* Entry 'Output0': '<S53>:130' */

20926
          ((int32_T *)ssGetDWork(S, 337))[0] = 0;

20927
        } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 1) {

20928
          /* Transition: '<S53>:161' */

20929
          ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input1;

20930


20931
          /* Entry 'Input1': '<S53>:165' */

20932
          ((int32_T *)ssGetDWork(S, 337))[0] = 0;

20933
        } else {

20934
          ((int32_T *)ssGetDWork(S, 337))[0] = AFCS_MODEL1_addup_m(1, 20,

20935
            ((int32_T *)ssGetDWork(S, 337))[0]);

20936
        }

20937
        break;

20938


20939
       case AFCS_MODEL1_IN_Input1:

20940
        /* During 'Input1': '<S53>:165' */

20941
        if (((int32_T *)ssGetDWork(S, 337))[0] == 20) {

20942
          /* Transition: '<S53>:168' */

20943
          ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Output1;

20944


20945
          /* Entry 'Output1': '<S53>:167' */

20946
          ((int32_T *)ssGetDWork(S, 337))[0] = 0;

20947
        } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 0) {

20948
          /* Transition: '<S53>:162' */

20949
          ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input0;

20950


20951
          /* Entry 'Input0': '<S53>:138' */

20952
          ((int32_T *)ssGetDWork(S, 337))[0] = 0;

20953
        } else {

20954
          ((int32_T *)ssGetDWork(S, 337))[0] = AFCS_MODEL1_addup_m(1, 20,

20955
            ((int32_T *)ssGetDWork(S, 337))[0]);

20956
        }

20957
        break;

20958


20959
       case AFCS_MODEL1_IN_Output0:

20960
        /* During 'Output0': '<S53>:130' */

20961
        if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 1) {

20962
          /* Transition: '<S53>:139' */

20963
          ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input1;

20964


20965
          /* Entry 'Input1': '<S53>:165' */

20966
          ((int32_T *)ssGetDWork(S, 337))[0] = 0;

20967
        }

20968
        break;

20969


20970
       default:

20971
        /* During 'Output1': '<S53>:167' */

20972
        if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 0) {

20973
          /* Transition: '<S53>:170' */

20974
          ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input0;

20975


20976
          /* Entry 'Input0': '<S53>:138' */

20977
          ((int32_T *)ssGetDWork(S, 337))[0] = 0;

20978
        }

20979
        break;

20980
      }

20981
    }

20982


20983
    /* End of Chart: '<S21>/delayblock6' */

20984


20985
    /* Chart: '<S299>/AirLineUpdateSucModel' */

20986
    /* Gateway: AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/AirLineUpdateSucModel */

20987
    /* During: AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/AirLineUpdateSucModel */

20988
    if (((uint8_T *)ssGetDWork(S, 588))[0] == 0U) {

20989
      /* Entry: AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/AirLineUpdateSucModel */

20990
      ((uint8_T *)ssGetDWork(S, 588))[0] = 1U;

20991


20992
      /* Entry Internal: AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/AirLineUpdateSucModel */

20993
      ((uint8_T *)ssGetDWork(S, 589))[0] = AFCS_M_IN_AirLineUpdateSucModel;

20994


20995
      /* Entry 'AirLineUpdateSucModel': '<S300>:38' */

20996
      /* Entry Internal 'AirLineUpdateSucModel': '<S300>:38' */

20997
      /* Transition: '<S300>:146' */

20998
      ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_pre;

20999


21000
      /* Entry 'pre': '<S300>:144' */

21001
      ((int8_T *)ssGetDWork(S, 515))[0] = 0;

21002
    } else {

21003
      /* During 'AirLineUpdateSucModel': '<S300>:38' */

21004
      switch (((uint8_T *)ssGetDWork(S, 590))[0]) {

21005
       case AFCS__IN_AirLineUpdateSucModel1:

21006
        /* During 'AirLineUpdateSucModel1': '<S300>:124' */

21007
        if (_rtB->airLineUpdateFlag == 0) {

21008
          /* Transition: '<S300>:138' */

21009
          ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_pre;

21010


21011
          /* Entry 'pre': '<S300>:144' */

21012
          ((int8_T *)ssGetDWork(S, 515))[0] = 0;

21013
        }

21014
        break;

21015


21016
       case AFCS_MODEL1_IN_Alt1:

21017
        /* During 'Alt1': '<S300>:140' */

21018
        if (((int8_T *)ssGetDWork(S, 515))[0] == 10) {

21019
          /* Transition: '<S300>:123' */

21020
          ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS__IN_AirLineUpdateSucModel1;

21021


21022
          /* Entry 'AirLineUpdateSucModel1': '<S300>:124' */

21023
        } else {

21024
          i_1 = ((int8_T *)ssGetDWork(S, 515))[0] + 1;

21025
          if (i_1 > 127) {

21026
            i_1 = 127;

21027
          }

21028


21029
          ((int8_T *)ssGetDWork(S, 515))[0] = (int8_T)i_1;

21030
        }

21031
        break;

21032


21033
       default:

21034
        /* During 'pre': '<S300>:144' */

21035
        if (_rtB->airLineUpdateFlag == 1) {

21036
          /* Transition: '<S300>:147' */

21037
          ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_Alt1;

21038


21039
          /* Entry 'Alt1': '<S300>:140' */

21040
          ((int8_T *)ssGetDWork(S, 515))[0] = 0;

21041
        }

21042
        break;

21043
      }

21044
    }

21045


21046
    /* End of Chart: '<S299>/AirLineUpdateSucModel' */

21047


21048
    /* RelationalOperator: '<S311>/Compare' incorporates:

21049
     *  Constant: '<S311>/Constant'

21050
     */

21051
    _rtB->Compare_p = (_rtB->fmphase >= 3);

21052


21053
    /* Delay: '<S310>/Delay' */

21054
    _rtB->RelationalOperator_l = ((boolean_T *)ssGetDWork(S, 502))[0];

21055


21056
    /* Logic: '<S310>/Logical Operator1' */

21057
    _rtB->LogicalOperator1 = _rtB->Compare_p ^ _rtB->RelationalOperator_l;

21058


21059
    /* UnitDelay: '<S309>/Unit Delay6' */

21060
    _rtB->UnitDelay6 = ((int8_T *)ssGetDWork(S, 365))[0];

21061


21062
    /* Chart: '<S29>/LLACalibration2' */

21063
    /* Gateway: AFCS_MODEL1/GNC/Subsystem1/LLACalibration2 */

21064
    /* During: AFCS_MODEL1/GNC/Subsystem1/LLACalibration2 */

21065
    if (((uint8_T *)ssGetDWork(S, 583))[0] == 0U) {

21066
      /* Entry: AFCS_MODEL1/GNC/Subsystem1/LLACalibration2 */

21067
      ((uint8_T *)ssGetDWork(S, 583))[0] = 1U;

21068


21069
      /* Entry Internal: AFCS_MODEL1/GNC/Subsystem1/LLACalibration2 */

21070
      /* Transition: '<S308>:146' */

21071
      ((uint8_T *)ssGetDWork(S, 584))[0] = AFCS_MODEL1_IN_AltCalibration;

21072


21073
      /* Entry 'AltCalibration': '<S308>:38' */

21074
      ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_pre;

21075


21076
      /* Entry 'pre': '<S308>:144' */

21077
      ((int32_T *)ssGetDWork(S, 331))[0] = 0;

21078
    } else {

21079
      /* During 'AltCalibration': '<S308>:38' */

21080
      switch (((uint8_T *)ssGetDWork(S, 585))[0]) {

21081
       case AFCS_MODEL1_IN_Alt1_o:

21082
        /* During 'Alt1': '<S308>:140' */

21083
        if (((int32_T *)ssGetDWork(S, 331))[0] == 10) {

21084
          /* Transition: '<S308>:123' */

21085
          ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_pre;

21086


21087
          /* Entry 'pre': '<S308>:144' */

21088
          ((int32_T *)ssGetDWork(S, 331))[0] = 0;

21089
        } else {

21090
          tmp_1 = ((int32_T *)ssGetDWork(S, 331))[0] + 1LL;

21091
          if (tmp_1 > 2147483647LL) {

21092
            tmp_1 = 2147483647LL;

21093
          } else {

21094
            if (tmp_1 < -2147483648LL) {

21095
              tmp_1 = -2147483648LL;

21096
            }

21097
          }

21098


21099
          ((int32_T *)ssGetDWork(S, 331))[0] = (int32_T)tmp_1;

21100
        }

21101
        break;

21102


21103
       case AFCS_MODEL1_IN_Alt2:

21104
        /* During 'Alt2': '<S308>:155' */

21105
        if (((int32_T *)ssGetDWork(S, 331))[0] == 10) {

21106
          /* Transition: '<S308>:157' */

21107
          ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_pre;

21108


21109
          /* Entry 'pre': '<S308>:144' */

21110
          ((int32_T *)ssGetDWork(S, 331))[0] = 0;

21111
        } else {

21112
          tmp_1 = ((int32_T *)ssGetDWork(S, 331))[0] + 1LL;

21113
          if (tmp_1 > 2147483647LL) {

21114
            tmp_1 = 2147483647LL;

21115
          } else {

21116
            if (tmp_1 < -2147483648LL) {

21117
              tmp_1 = -2147483648LL;

21118
            }

21119
          }

21120


21121
          ((int32_T *)ssGetDWork(S, 331))[0] = (int32_T)tmp_1;

21122
        }

21123
        break;

21124


21125
       default:

21126
        /* During 'pre': '<S308>:144' */

21127
        if (_rtB->LogicalOperator1) {

21128
          /* Transition: '<S308>:147' */

21129
          ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_Alt1_o;

21130


21131
          /* Entry 'Alt1': '<S308>:140' */

21132
          ((int32_T *)ssGetDWork(S, 331))[0] = 0;

21133
        } else {

21134
          if (_rtB->UnitDelay6 == 1) {

21135
            /* Transition: '<S308>:156' */

21136
            ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_Alt2;

21137


21138
            /* Entry 'Alt2': '<S308>:155' */

21139
            ((int32_T *)ssGetDWork(S, 331))[0] = 0;

21140
          }

21141
        }

21142
        break;

21143
      }

21144
    }

21145


21146
    /* End of Chart: '<S29>/LLACalibration2' */

21147


21148
    /* UnitDelay: '<S309>/Unit Delay5' */

21149
    _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 366))[0];

21150


21151
    /* RelationalOperator: '<S309>/Relational Operator' incorporates:

21152
     *  Constant: '<S7>/Constant'

21153
     */

21154
    _rtB->RelationalOperator_l = (_rtB->UnitDelay5_b != 0);

21155


21156
    /* DataTypeConversion: '<S309>/Data Type Conversion2' */

21157
    _rtB->DataTypeConversion2_l = (int8_T)_rtB->RelationalOperator_l;

21158
  }

21159


21160
  if (1) {

21161
    /* Gain: '<S9>/Gain' */

21162
    _rtB->Gain = 57.3 * _rtB->psic;

21163
  }

21164


21165
  if (ssIsSampleHit(S, 1, 0)) {

21166
  }

21167


21168
  if (1) {

21169
    /* Gain: '<S337>/Unit Conversion' */

21170
    _rtB->UnaryMinus = 0.017453292519943295 * _rtB->LLA0[1];

21171


21172
    /* Gain: '<S332>/Gain' */

21173
    _rtB->Switch_dn = -_rtB->LLA0[0];

21174


21175
    /* Sum: '<S332>/Sum' incorporates:

21176
     *  Constant: '<S332>/Constant1'

21177
     */

21178
    _rtB->Switch_dn += -90.0;

21179


21180
    /* Gain: '<S338>/Unit Conversion' */

21181
    _rtB->Switch_dn *= 0.017453292519943295;

21182


21183
    /* SignalConversion: '<S341>/TmpSignal ConversionAtsincosInport1' incorporates:

21184
     *  Constant: '<S332>/Constant4'

21185
     */

21186
    _rtB->Sum1_m[0] = _rtB->UnaryMinus;

21187
    _rtB->Sum1_m[1] = _rtB->Switch_dn;

21188
    _rtB->Sum1_m[2] = 0.0;

21189


21190
    /* Trigonometry: '<S341>/sincos' */

21191
    TrigonometricFunction4_o = cos(_rtB->Sum1_m[0]);

21192
    _rtB->Sum1_m[0] = sin(_rtB->Sum1_m[0]);

21193
    _rtB->Sum_o[0] = TrigonometricFunction4_o;

21194
    TrigonometricFunction4_o = cos(_rtB->Sum1_m[1]);

21195
    _rtB->Sum1_m[1] = sin(_rtB->Sum1_m[1]);

21196
    _rtB->Sum_o[1] = TrigonometricFunction4_o;

21197
    TrigonometricFunction4_o = cos(_rtB->Sum1_m[2]);

21198
    _rtB->Sum1_m[2] = sin(_rtB->Sum1_m[2]);

21199
    _rtB->Sum_o[2] = TrigonometricFunction4_o;

21200


21201
    /* Fcn: '<S341>/Fcn11' */

21202
    _rtB->VectorConcatenate_m[0] = _rtB->Sum_o[1] * _rtB->Sum_o[0];

21203


21204
    /* Fcn: '<S341>/Fcn21' */

21205
    _rtB->VectorConcatenate_m[1] = _rtB->Sum1_m[2] * _rtB->Sum1_m[1] *

21206
      _rtB->Sum_o[0] - _rtB->Sum_o[2] * _rtB->Sum1_m[0];

21207


21208
    /* Fcn: '<S341>/Fcn31' */

21209
    _rtB->VectorConcatenate_m[2] = _rtB->Sum_o[2] * _rtB->Sum1_m[1] *

21210
      _rtB->Sum_o[0] + _rtB->Sum1_m[2] * _rtB->Sum1_m[0];

21211


21212
    /* Fcn: '<S341>/Fcn12' */

21213
    _rtB->VectorConcatenate_m[3] = _rtB->Sum_o[1] * _rtB->Sum1_m[0];

21214


21215
    /* Fcn: '<S341>/Fcn22' */

21216
    _rtB->VectorConcatenate_m[4] = _rtB->Sum1_m[2] * _rtB->Sum1_m[1] *

21217
      _rtB->Sum1_m[0] + _rtB->Sum_o[2] * _rtB->Sum_o[0];

21218


21219
    /* Fcn: '<S341>/Fcn32' */

21220
    _rtB->VectorConcatenate_m[5] = _rtB->Sum_o[2] * _rtB->Sum1_m[1] *

21221
      _rtB->Sum1_m[0] - _rtB->Sum1_m[2] * _rtB->Sum_o[0];

21222


21223
    /* Fcn: '<S341>/Fcn13' */

21224
    _rtB->VectorConcatenate_m[6] = -_rtB->Sum1_m[1];

21225


21226
    /* Fcn: '<S341>/Fcn23' */

21227
    _rtB->VectorConcatenate_m[7] = _rtB->Sum1_m[2] * _rtB->Sum_o[1];

21228


21229
    /* Fcn: '<S341>/Fcn33' */

21230
    _rtB->VectorConcatenate_m[8] = _rtB->Sum_o[2] * _rtB->Sum_o[1];

21231


21232
    /* Abs: '<S418>/Abs' */

21233
    _rtB->UnaryMinus = fabs(_rtB->Lat);

21234


21235
    /* RelationalOperator: '<S419>/Compare' incorporates:

21236
     *  Constant: '<S419>/Constant'

21237
     */

21238
    _rtB->Compare_in = (_rtB->UnaryMinus > 180.0);

21239


21240
    /* Switch: '<S418>/Switch' */

21241
    if (_rtB->Compare_in) {

21242
      /* Bias: '<S418>/Bias' */

21243
      _rtB->MathFunction1_c = _rtB->Lat + 180.0;

21244


21245
      /* Math: '<S418>/Math Function1' incorporates:

21246
       *  Constant: '<S418>/Constant2'

21247
       */

21248
      _rtB->MathFunction1_c = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_c,

21249
        360.0);

21250


21251
      /* Bias: '<S418>/Bias1' */

21252
      _rtB->Bias1_p = _rtB->MathFunction1_c + -180.0;

21253
      _rtB->Switch = _rtB->Bias1_p;

21254
    } else {

21255
      _rtB->Switch = _rtB->Lat;

21256
    }

21257


21258
    /* End of Switch: '<S418>/Switch' */

21259


21260
    /* Abs: '<S415>/Abs1' */

21261
    _rtB->Abs1 = fabs(_rtB->Switch);

21262


21263
    /* RelationalOperator: '<S417>/Compare' incorporates:

21264
     *  Constant: '<S417>/Constant'

21265
     */

21266
    _rtB->Compare_in = (_rtB->Abs1 > 90.0);

21267


21268
    /* Switch: '<S415>/Switch' */

21269
    if (_rtB->Compare_in) {

21270
      /* Bias: '<S415>/Bias' */

21271
      _rtB->Bias1_pq = _rtB->Abs1 + -90.0;

21272


21273
      /* Gain: '<S415>/Gain' */

21274
      _rtB->Bias1_pq = -_rtB->Bias1_pq;

21275


21276
      /* Bias: '<S415>/Bias1' */

21277
      _rtB->Bias1_pq += 90.0;

21278


21279
      /* Signum: '<S415>/Sign1' */

21280
      if (_rtB->Switch < 0.0) {

21281
        _rtB->Sign1_g = -1.0;

21282
      } else if (_rtB->Switch > 0.0) {

21283
        _rtB->Sign1_g = 1.0;

21284
      } else if (_rtB->Switch == 0.0) {

21285
        _rtB->Sign1_g = 0.0;

21286
      } else {

21287
        _rtB->Sign1_g = _rtB->Switch;

21288
      }

21289


21290
      /* End of Signum: '<S415>/Sign1' */

21291


21292
      /* Product: '<S415>/Divide1' */

21293
      _rtB->Divide1_o = _rtB->Sign1_g * _rtB->Bias1_pq;

21294
      _rtB->UnaryMinus = _rtB->Divide1_o;

21295
    } else {

21296
      _rtB->UnaryMinus = _rtB->Switch;

21297
    }

21298


21299
    /* End of Switch: '<S415>/Switch' */

21300


21301
    /* Gain: '<S409>/Unit Conversion' */

21302
    _rtB->Switch_dn = 0.017453292519943295 * _rtB->UnaryMinus;

21303


21304
    /* Trigonometry: '<S384>/sincos1' */

21305
    _rtB->u2u3_a = sin(_rtB->Switch_dn);

21306
    _rtB->Add_em = cos(_rtB->Switch_dn);

21307


21308
    /* Product: '<S384>/Product3' incorporates:

21309
     *  Constant: '<S340>/Constant2'

21310
     */

21311
    _rtB->sincos_o2_j[0] = 0.0 * _rtB->u2u3_a;

21312
    _rtB->sincos_o2_j[1] = 0.0 * _rtB->Add_em;

21313


21314
    /* Trigonometry: '<S384>/Trigonometric Function1' */

21315
    _rtB->Switch_dn = tan(_rtB->Switch_dn);

21316


21317
    /* Product: '<S384>/Product2' */

21318
    _rtB->Switch_dn = _rtB->Switch_dn * AFCS_MODEL1_rtC(S)->Sum *

21319
      AFCS_MODEL1_rtC(S)->Sum;

21320


21321
    /* Trigonometry: '<S384>/Trigonometric Function2' incorporates:

21322
     *  Constant: '<S384>/f'

21323
     */

21324
    _rtB->Switch_dn = AFCS_MODEL1_rt_atan2d_snf(_rtB->Switch_dn, 1.0);

21325


21326
    /* Gain: '<S411>/Unit Conversion' */

21327
    _rtB->Add_em = 57.295779513082323 * _rtB->Switch_dn;

21328


21329
    /* Gain: '<S414>/Unit Conversion' */

21330
    _rtB->Add_em *= 0.017453292519943295;

21331


21332
    /* Trigonometry: '<S413>/Trigonometric Function' */

21333
    _rtB->Add_em = sin(_rtB->Add_em);

21334


21335
    /* Product: '<S413>/Product2' */

21336
    _rtB->Add_em = _rtB->Add_em * _rtB->Add_em * AFCS_MODEL1_rtC(S)->Sum1_i;

21337


21338
    /* Sum: '<S413>/Sum2' incorporates:

21339
     *  Constant: '<S413>/Constant'

21340
     */

21341
    _rtB->Add_em++;

21342


21343
    /* Product: '<S413>/Product3' incorporates:

21344
     *  Constant: '<S413>/Re'

21345
     */

21346
    _rtB->Add_em = 4.0680631590769E+13 / _rtB->Add_em;

21347


21348
    /* Sqrt: '<S413>/sqrt' */

21349
    _rtB->Add_em = sqrt(_rtB->Add_em);

21350


21351
    /* Trigonometry: '<S384>/sincos' */

21352
    _rtB->u2u3_a = cos(_rtB->Switch_dn);

21353
    _rtB->Switch_dn = sin(_rtB->Switch_dn);

21354


21355
    /* Product: '<S384>/Product1' */

21356
    _rtB->sincos_o1_d[0] = _rtB->Add_em * _rtB->Switch_dn;

21357
    _rtB->sincos_o1_d[1] = _rtB->Add_em * _rtB->u2u3_a;

21358


21359
    /* Sum: '<S384>/Sum1' */

21360
    _rtB->sincos_o2_j[0] += _rtB->sincos_o1_d[0];

21361
    _rtB->sincos_o2_j[1] += _rtB->sincos_o1_d[1];

21362


21363
    /* Trigonometry: '<S384>/Trigonometric Function3' */

21364
    _rtB->Add_em = AFCS_MODEL1_rt_atan2d_snf(_rtB->sincos_o2_j[0],

21365
      _rtB->sincos_o2_j[1]);

21366


21367
    /* Gain: '<S410>/Unit Conversion' */

21368
    _rtB->Add_em *= 57.295779513082323;

21369


21370
    /* Switch: '<S385>/Switch1' incorporates:

21371
     *  Constant: '<S385>/Constant'

21372
     *  Constant: '<S385>/Constant1'

21373
     */

21374
    if (_rtB->Compare_in) {

21375
      _rtB->Switch_dn = 180.0;

21376
    } else {

21377
      _rtB->Switch_dn = 0.0;

21378
    }

21379


21380
    /* End of Switch: '<S385>/Switch1' */

21381


21382
    /* Sum: '<S385>/Sum' */

21383
    _rtB->Sum = _rtB->Switch_dn + _rtB->Lon;

21384


21385
    /* Abs: '<S416>/Abs' */

21386
    _rtB->Switch_dn = fabs(_rtB->Sum);

21387


21388
    /* RelationalOperator: '<S420>/Compare' incorporates:

21389
     *  Constant: '<S420>/Constant'

21390
     */

21391
    _rtB->Compare_in = (_rtB->Switch_dn > 180.0);

21392


21393
    /* Switch: '<S416>/Switch' */

21394
    if (_rtB->Compare_in) {

21395
      /* Bias: '<S416>/Bias' */

21396
      _rtB->MathFunction1_e = _rtB->Sum + 180.0;

21397


21398
      /* Math: '<S416>/Math Function1' incorporates:

21399
       *  Constant: '<S416>/Constant2'

21400
       */

21401
      _rtB->MathFunction1_e = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_e,

21402
        360.0);

21403


21404
      /* Bias: '<S416>/Bias1' */

21405
      _rtB->Bias1_a = _rtB->MathFunction1_e + -180.0;

21406
      _rtB->Switch_dn = _rtB->Bias1_a;

21407
    } else {

21408
      _rtB->Switch_dn = _rtB->Sum;

21409
    }

21410


21411
    /* End of Switch: '<S416>/Switch' */

21412


21413
    /* Gain: '<S407>/Unit Conversion' */

21414
    _rtB->sincos_o1_d[0] = 0.017453292519943295 * _rtB->Add_em;

21415
    _rtB->sincos_o1_d[1] = 0.017453292519943295 * _rtB->Switch_dn;

21416


21417
    /* Trigonometry: '<S383>/sincos' */

21418
    TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[0]);

21419
    _rtB->sincos_o1_d[0] = sin(_rtB->sincos_o1_d[0]);

21420
    _rtB->sincos_o2_j[0] = TrigonometricFunction4_o;

21421
    TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[1]);

21422
    _rtB->sincos_o1_d[1] = sin(_rtB->sincos_o1_d[1]);

21423
    _rtB->sincos_o2_j[1] = TrigonometricFunction4_o;

21424


21425
    /* Product: '<S398>/u(1)*u(4)' */

21426
    _rtB->u2u3_a = _rtB->sincos_o1_d[0] * _rtB->sincos_o2_j[1];

21427


21428
    /* UnaryMinus: '<S398>/Unary Minus' */

21429
    _rtB->VectorConcatenate_e[0] = -_rtB->u2u3_a;

21430


21431
    /* UnaryMinus: '<S401>/Unary Minus' */

21432
    _rtB->VectorConcatenate_e[1] = -_rtB->sincos_o1_d[1];

21433


21434
    /* Product: '<S404>/u(3)*u(4)' */

21435
    _rtB->u2u3_a = _rtB->sincos_o2_j[0] * _rtB->sincos_o2_j[1];

21436


21437
    /* UnaryMinus: '<S404>/Unary Minus' */

21438
    _rtB->VectorConcatenate_e[2] = -_rtB->u2u3_a;

21439


21440
    /* Product: '<S399>/u(1)*u(2)' */

21441
    _rtB->u2u3_a = _rtB->sincos_o1_d[0] * _rtB->sincos_o1_d[1];

21442


21443
    /* UnaryMinus: '<S399>/Unary Minus' */

21444
    _rtB->VectorConcatenate_e[3] = -_rtB->u2u3_a;

21445


21446
    /* SignalConversion: '<S408>/ConcatBufferAtVector ConcatenateIn5' */

21447
    _rtB->VectorConcatenate_e[4] = _rtB->sincos_o2_j[1];

21448


21449
    /* Product: '<S405>/u(2)*u(3)' */

21450
    _rtB->u2u3_a = _rtB->sincos_o1_d[1] * _rtB->sincos_o2_j[0];

21451


21452
    /* UnaryMinus: '<S405>/Unary Minus' */

21453
    _rtB->VectorConcatenate_e[5] = -_rtB->u2u3_a;

21454


21455
    /* SignalConversion: '<S408>/ConcatBufferAtVector ConcatenateIn7' */

21456
    _rtB->VectorConcatenate_e[6] = _rtB->sincos_o2_j[0];

21457


21458
    /* SignalConversion: '<S408>/ConcatBufferAtVector ConcatenateIn8' incorporates:

21459
     *  Constant: '<S403>/Constant'

21460
     */

21461
    _rtB->VectorConcatenate_e[7] = 0.0;

21462


21463
    /* UnaryMinus: '<S406>/Unary Minus' */

21464
    _rtB->VectorConcatenate_e[8] = -_rtB->sincos_o1_d[0];

21465


21466
    /* Math: '<S340>/Math Function1' */

21467
    for (i_1 = 0; i_1 < 3; i_1++) {

21468
      _rtB->MathFunction_kc[3 * i_1] = _rtB->VectorConcatenate_e[i_1];

21469
      _rtB->MathFunction_kc[1 + 3 * i_1] = _rtB->VectorConcatenate_e[i_1 + 3];

21470
      _rtB->MathFunction_kc[2 + 3 * i_1] = _rtB->VectorConcatenate_e[i_1 + 6];

21471
    }

21472


21473
    /* End of Math: '<S340>/Math Function1' */

21474


21475
    /* Gain: '<S421>/Unit Conversion' */

21476
    _rtB->Add_em *= 0.017453292519943295;

21477


21478
    /* Trigonometry: '<S386>/Trigonometric Function' */

21479
    _rtB->Add_em = sin(_rtB->Add_em);

21480


21481
    /* Product: '<S386>/Product2' */

21482
    _rtB->Add_em = _rtB->Add_em * _rtB->Add_em * AFCS_MODEL1_rtC(S)->Sum1_a;

21483


21484
    /* Sum: '<S386>/Sum2' incorporates:

21485
     *  Constant: '<S386>/Constant'

21486
     */

21487
    _rtB->Add_em++;

21488


21489
    /* Product: '<S386>/Product3' incorporates:

21490
     *  Constant: '<S386>/Re'

21491
     */

21492
    _rtB->Add_em = 4.0680631590769E+13 / _rtB->Add_em;

21493


21494
    /* Sqrt: '<S386>/sqrt' */

21495
    _rtB->Add_em = sqrt(_rtB->Add_em);

21496


21497
    /* UnaryMinus: '<S340>/Unary Minus' */

21498
    _rtB->Add_em = -_rtB->Add_em;

21499


21500
    /* SignalConversion: '<S340>/TmpSignal ConversionAtProduct1Inport2' incorporates:

21501
     *  Constant: '<S340>/Constant'

21502
     */

21503
    _rtB->Sum_o[0] = 0.0;

21504
    _rtB->Sum_o[1] = 0.0;

21505
    _rtB->Sum_o[2] = _rtB->Add_em;

21506


21507
    /* Product: '<S340>/Product1' */

21508
    for (i_1 = 0; i_1 < 3; i_1++) {

21509
      _rtB->Sum1_m[i_1] = 0.0;

21510
      _rtB->Sum1_m[i_1] += _rtB->MathFunction_kc[i_1] * _rtB->Sum_o[0];

21511
      _rtB->Sum1_m[i_1] += _rtB->MathFunction_kc[i_1 + 3] * _rtB->Sum_o[1];

21512
      _rtB->Sum1_m[i_1] += _rtB->MathFunction_kc[i_1 + 6] * _rtB->Sum_o[2];

21513
    }

21514


21515
    /* End of Product: '<S340>/Product1' */

21516


21517
    /* Gain: '<S396>/Unit Conversion' */

21518
    _rtB->sincos_o1_d[0] = 0.017453292519943295 * _rtB->UnaryMinus;

21519
    _rtB->sincos_o1_d[1] = 0.017453292519943295 * _rtB->Switch_dn;

21520


21521
    /* Trigonometry: '<S382>/sincos' */

21522
    TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[0]);

21523
    _rtB->sincos_o1_d[0] = sin(_rtB->sincos_o1_d[0]);

21524
    _rtB->sincos_o2_j[0] = TrigonometricFunction4_o;

21525
    TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[1]);

21526
    _rtB->sincos_o1_d[1] = sin(_rtB->sincos_o1_d[1]);

21527
    _rtB->sincos_o2_j[1] = TrigonometricFunction4_o;

21528


21529
    /* Product: '<S387>/u(1)*u(4)' */

21530
    _rtB->Add_em = _rtB->sincos_o1_d[0] * _rtB->sincos_o2_j[1];

21531


21532
    /* UnaryMinus: '<S387>/Unary Minus' */

21533
    _rtB->VectorConcatenate_bv[0] = -_rtB->Add_em;

21534


21535
    /* UnaryMinus: '<S390>/Unary Minus' */

21536
    _rtB->VectorConcatenate_bv[1] = -_rtB->sincos_o1_d[1];

21537


21538
    /* Product: '<S393>/u(3)*u(4)' */

21539
    _rtB->Add_em = _rtB->sincos_o2_j[0] * _rtB->sincos_o2_j[1];

21540


21541
    /* UnaryMinus: '<S393>/Unary Minus' */

21542
    _rtB->VectorConcatenate_bv[2] = -_rtB->Add_em;

21543


21544
    /* Product: '<S388>/u(1)*u(2)' */

21545
    _rtB->Add_em = _rtB->sincos_o1_d[0] * _rtB->sincos_o1_d[1];

21546


21547
    /* UnaryMinus: '<S388>/Unary Minus' */

21548
    _rtB->VectorConcatenate_bv[3] = -_rtB->Add_em;

21549


21550
    /* SignalConversion: '<S397>/ConcatBufferAtVector ConcatenateIn5' */

21551
    _rtB->VectorConcatenate_bv[4] = _rtB->sincos_o2_j[1];

21552


21553
    /* Product: '<S394>/u(2)*u(3)' */

21554
    _rtB->Add_em = _rtB->sincos_o1_d[1] * _rtB->sincos_o2_j[0];

21555


21556
    /* UnaryMinus: '<S394>/Unary Minus' */

21557
    _rtB->VectorConcatenate_bv[5] = -_rtB->Add_em;

21558


21559
    /* SignalConversion: '<S397>/ConcatBufferAtVector ConcatenateIn7' */

21560
    _rtB->VectorConcatenate_bv[6] = _rtB->sincos_o2_j[0];

21561


21562
    /* SignalConversion: '<S397>/ConcatBufferAtVector ConcatenateIn8' incorporates:

21563
     *  Constant: '<S392>/Constant'

21564
     */

21565
    _rtB->VectorConcatenate_bv[7] = 0.0;

21566


21567
    /* UnaryMinus: '<S395>/Unary Minus' */

21568
    _rtB->VectorConcatenate_bv[8] = -_rtB->sincos_o1_d[0];

21569


21570
    /* Math: '<S340>/Math Function' */

21571
    for (i_1 = 0; i_1 < 3; i_1++) {

21572
      _rtB->MathFunction_kc[3 * i_1] = _rtB->VectorConcatenate_bv[i_1];

21573
      _rtB->MathFunction_kc[1 + 3 * i_1] = _rtB->VectorConcatenate_bv[i_1 + 3];

21574
      _rtB->MathFunction_kc[2 + 3 * i_1] = _rtB->VectorConcatenate_bv[i_1 + 6];

21575
    }

21576


21577
    /* End of Math: '<S340>/Math Function' */

21578


21579
    /* UnaryMinus: '<S340>/Unary Minus1' */

21580
    _rtB->Add_em = -_rtB->Altitude;

21581


21582
    /* SignalConversion: '<S340>/TmpSignal ConversionAtProductInport2' incorporates:

21583
     *  Constant: '<S340>/Constant1'

21584
     */

21585
    _rtB->Sum_o[0] = 0.0;

21586
    _rtB->Sum_o[1] = 0.0;

21587
    _rtB->Sum_o[2] = _rtB->Add_em;

21588
    for (i_1 = 0; i_1 < 3; i_1++) {

21589
      /* Product: '<S340>/Product' */

21590
      _rtB->TmpSignalConversionAtProductI_c[i_1] = 0.0;

21591
      _rtB->TmpSignalConversionAtProductI_c[i_1] += _rtB->MathFunction_kc[i_1] *

21592
        _rtB->Sum_o[0];

21593
      _rtB->TmpSignalConversionAtProductI_c[i_1] += _rtB->MathFunction_kc[i_1 +

21594
        3] * _rtB->Sum_o[1];

21595
      _rtB->TmpSignalConversionAtProductI_c[i_1] += _rtB->MathFunction_kc[i_1 +

21596
        6] * _rtB->Sum_o[2];

21597


21598
      /* Sum: '<S340>/Sum' */

21599
      _rtB->Sum1_m[i_1] += _rtB->TmpSignalConversionAtProductI_c[i_1];

21600
    }

21601


21602
    /* Abs: '<S378>/Abs' */

21603
    _rtB->Add_em = fabs(_rtB->LLA0[0]);

21604


21605
    /* RelationalOperator: '<S379>/Compare' incorporates:

21606
     *  Constant: '<S379>/Constant'

21607
     */

21608
    _rtB->Compare_in = (_rtB->Add_em > 180.0);

21609


21610
    /* Switch: '<S378>/Switch' */

21611
    if (_rtB->Compare_in) {

21612
      /* Bias: '<S378>/Bias' */

21613
      _rtB->MathFunction1_d = _rtB->LLA0[0] + 180.0;

21614


21615
      /* Math: '<S378>/Math Function1' incorporates:

21616
       *  Constant: '<S378>/Constant2'

21617
       */

21618
      _rtB->MathFunction1_d = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_d,

21619
        360.0);

21620


21621
      /* Bias: '<S378>/Bias1' */

21622
      _rtB->Bias1_b = _rtB->MathFunction1_d + -180.0;

21623
      _rtB->Switch_l = _rtB->Bias1_b;

21624
    } else {

21625
      _rtB->Switch_l = _rtB->LLA0[0];

21626
    }

21627


21628
    /* End of Switch: '<S378>/Switch' */

21629


21630
    /* Abs: '<S375>/Abs1' */

21631
    _rtB->Abs1_l = fabs(_rtB->Switch_l);

21632


21633
    /* RelationalOperator: '<S377>/Compare' incorporates:

21634
     *  Constant: '<S377>/Constant'

21635
     */

21636
    _rtB->Compare_in = (_rtB->Abs1_l > 90.0);

21637


21638
    /* Switch: '<S375>/Switch' */

21639
    if (_rtB->Compare_in) {

21640
      /* Bias: '<S375>/Bias' */

21641
      _rtB->Bias1_ca = _rtB->Abs1_l + -90.0;

21642


21643
      /* Gain: '<S375>/Gain' */

21644
      _rtB->Bias1_ca = -_rtB->Bias1_ca;

21645


21646
      /* Bias: '<S375>/Bias1' */

21647
      _rtB->Bias1_ca += 90.0;

21648


21649
      /* Signum: '<S375>/Sign1' */

21650
      if (_rtB->Switch_l < 0.0) {

21651
        _rtB->Sign1_o = -1.0;

21652
      } else if (_rtB->Switch_l > 0.0) {

21653
        _rtB->Sign1_o = 1.0;

21654
      } else if (_rtB->Switch_l == 0.0) {

21655
        _rtB->Sign1_o = 0.0;

21656
      } else {

21657
        _rtB->Sign1_o = _rtB->Switch_l;

21658
      }

21659


21660
      /* End of Signum: '<S375>/Sign1' */

21661


21662
      /* Product: '<S375>/Divide1' */

21663
      _rtB->Divide1_c = _rtB->Sign1_o * _rtB->Bias1_ca;

21664
      _rtB->Add_em = _rtB->Divide1_c;

21665
    } else {

21666
      _rtB->Add_em = _rtB->Switch_l;

21667
    }

21668


21669
    /* End of Switch: '<S375>/Switch' */

21670


21671
    /* Gain: '<S369>/Unit Conversion' */

21672
    _rtB->UnaryMinus = 0.017453292519943295 * _rtB->Add_em;

21673


21674
    /* Trigonometry: '<S344>/sincos1' */

21675
    _rtB->Switch_dn = sin(_rtB->UnaryMinus);

21676
    _rtB->u2u3_a = cos(_rtB->UnaryMinus);

21677


21678
    /* Product: '<S344>/Product3' incorporates:

21679
     *  Constant: '<S339>/Constant2'

21680
     */

21681
    _rtB->sincos_o1_d[0] = 0.0 * _rtB->Switch_dn;

21682
    _rtB->sincos_o1_d[1] = 0.0 * _rtB->u2u3_a;

21683


21684
    /* Trigonometry: '<S344>/Trigonometric Function1' */

21685
    _rtB->UnaryMinus = tan(_rtB->UnaryMinus);

21686


21687
    /* Product: '<S344>/Product2' */

21688
    _rtB->UnaryMinus = _rtB->UnaryMinus * AFCS_MODEL1_rtC(S)->Sum_dc *

21689
      AFCS_MODEL1_rtC(S)->Sum_dc;

21690


21691
    /* Trigonometry: '<S344>/Trigonometric Function2' incorporates:

21692
     *  Constant: '<S344>/f'

21693
     */

21694
    _rtB->UnaryMinus = AFCS_MODEL1_rt_atan2d_snf(_rtB->UnaryMinus, 1.0);

21695


21696
    /* Gain: '<S371>/Unit Conversion' */

21697
    _rtB->Switch_dn = 57.295779513082323 * _rtB->UnaryMinus;

21698


21699
    /* Gain: '<S374>/Unit Conversion' */

21700
    _rtB->Switch_dn *= 0.017453292519943295;

21701


21702
    /* Trigonometry: '<S373>/Trigonometric Function' */

21703
    _rtB->Switch_dn = sin(_rtB->Switch_dn);

21704


21705
    /* Product: '<S373>/Product2' */

21706
    _rtB->Switch_dn = _rtB->Switch_dn * _rtB->Switch_dn * AFCS_MODEL1_rtC(S)

21707
      ->Sum1_f;

21708


21709
    /* Sum: '<S373>/Sum2' incorporates:

21710
     *  Constant: '<S373>/Constant'

21711
     */

21712
    _rtB->Switch_dn++;

21713


21714
    /* Product: '<S373>/Product3' incorporates:

21715
     *  Constant: '<S373>/Re'

21716
     */

21717
    _rtB->Switch_dn = 4.0680631590769E+13 / _rtB->Switch_dn;

21718


21719
    /* Sqrt: '<S373>/sqrt' */

21720
    _rtB->Switch_dn = sqrt(_rtB->Switch_dn);

21721


21722
    /* Trigonometry: '<S344>/sincos' */

21723
    _rtB->u2u3_a = cos(_rtB->UnaryMinus);

21724
    _rtB->UnaryMinus = sin(_rtB->UnaryMinus);

21725


21726
    /* Product: '<S344>/Product1' */

21727
    _rtB->sincos_o2_j[0] = _rtB->Switch_dn * _rtB->UnaryMinus;

21728
    _rtB->sincos_o2_j[1] = _rtB->Switch_dn * _rtB->u2u3_a;

21729


21730
    /* Sum: '<S344>/Sum1' */

21731
    _rtB->sincos_o1_d[0] += _rtB->sincos_o2_j[0];

21732
    _rtB->sincos_o1_d[1] += _rtB->sincos_o2_j[1];

21733


21734
    /* Trigonometry: '<S344>/Trigonometric Function3' */

21735
    _rtB->UnaryMinus = AFCS_MODEL1_rt_atan2d_snf(_rtB->sincos_o1_d[0],

21736
      _rtB->sincos_o1_d[1]);

21737


21738
    /* Gain: '<S370>/Unit Conversion' */

21739
    _rtB->UnaryMinus *= 57.295779513082323;

21740


21741
    /* Switch: '<S345>/Switch1' incorporates:

21742
     *  Constant: '<S345>/Constant'

21743
     *  Constant: '<S345>/Constant1'

21744
     */

21745
    if (_rtB->Compare_in) {

21746
      _rtB->Switch_dn = 180.0;

21747
    } else {

21748
      _rtB->Switch_dn = 0.0;

21749
    }

21750


21751
    /* End of Switch: '<S345>/Switch1' */

21752


21753
    /* Sum: '<S345>/Sum' */

21754
    _rtB->Sum_j = _rtB->Switch_dn + _rtB->LLA0[1];

21755


21756
    /* Abs: '<S376>/Abs' */

21757
    _rtB->Switch_dn = fabs(_rtB->Sum_j);

21758


21759
    /* RelationalOperator: '<S380>/Compare' incorporates:

21760
     *  Constant: '<S380>/Constant'

21761
     */

21762
    _rtB->Compare_in = (_rtB->Switch_dn > 180.0);

21763


21764
    /* Switch: '<S376>/Switch' */

21765
    if (_rtB->Compare_in) {

21766
      /* Bias: '<S376>/Bias' */

21767
      _rtB->MathFunction1_f = _rtB->Sum_j + 180.0;

21768


21769
      /* Math: '<S376>/Math Function1' incorporates:

21770
       *  Constant: '<S376>/Constant2'

21771
       */

21772
      _rtB->MathFunction1_f = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_f,

21773
        360.0);

21774


21775
      /* Bias: '<S376>/Bias1' */

21776
      _rtB->Bias1_c = _rtB->MathFunction1_f + -180.0;

21777
      _rtB->Switch_dn = _rtB->Bias1_c;

21778
    } else {

21779
      _rtB->Switch_dn = _rtB->Sum_j;

21780
    }

21781


21782
    /* End of Switch: '<S376>/Switch' */

21783


21784
    /* Gain: '<S367>/Unit Conversion' */

21785
    _rtB->sincos_o1_d[0] = 0.017453292519943295 * _rtB->UnaryMinus;

21786
    _rtB->sincos_o1_d[1] = 0.017453292519943295 * _rtB->Switch_dn;

21787


21788
    /* Trigonometry: '<S343>/sincos' */

21789
    TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[0]);

21790
    _rtB->sincos_o1_d[0] = sin(_rtB->sincos_o1_d[0]);

21791
    _rtB->sincos_o2_j[0] = TrigonometricFunction4_o;

21792
    TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[1]);

21793
    _rtB->sincos_o1_d[1] = sin(_rtB->sincos_o1_d[1]);

21794
    _rtB->sincos_o2_j[1] = TrigonometricFunction4_o;

21795


21796
    /* Product: '<S358>/u(1)*u(4)' */

21797
    _rtB->u2u3_a = _rtB->sincos_o1_d[0] * _rtB->sincos_o2_j[1];

21798


21799
    /* UnaryMinus: '<S358>/Unary Minus' */

21800
    _rtB->VectorConcatenate_i[0] = -_rtB->u2u3_a;

21801


21802
    /* UnaryMinus: '<S361>/Unary Minus' */

21803
    _rtB->VectorConcatenate_i[1] = -_rtB->sincos_o1_d[1];

21804


21805
    /* Product: '<S364>/u(3)*u(4)' */

21806
    _rtB->u2u3_a = _rtB->sincos_o2_j[0] * _rtB->sincos_o2_j[1];

21807


21808
    /* UnaryMinus: '<S364>/Unary Minus' */

21809
    _rtB->VectorConcatenate_i[2] = -_rtB->u2u3_a;

21810


21811
    /* Product: '<S359>/u(1)*u(2)' */

21812
    _rtB->u2u3_a = _rtB->sincos_o1_d[0] * _rtB->sincos_o1_d[1];

21813


21814
    /* UnaryMinus: '<S359>/Unary Minus' */

21815
    _rtB->VectorConcatenate_i[3] = -_rtB->u2u3_a;

21816


21817
    /* SignalConversion: '<S368>/ConcatBufferAtVector ConcatenateIn5' */

21818
    _rtB->VectorConcatenate_i[4] = _rtB->sincos_o2_j[1];

21819


21820
    /* Product: '<S365>/u(2)*u(3)' */

21821
    _rtB->u2u3_a = _rtB->sincos_o1_d[1] * _rtB->sincos_o2_j[0];

21822


21823
    /* UnaryMinus: '<S365>/Unary Minus' */

21824
    _rtB->VectorConcatenate_i[5] = -_rtB->u2u3_a;

21825


21826
    /* SignalConversion: '<S368>/ConcatBufferAtVector ConcatenateIn7' */

21827
    _rtB->VectorConcatenate_i[6] = _rtB->sincos_o2_j[0];

21828


21829
    /* SignalConversion: '<S368>/ConcatBufferAtVector ConcatenateIn8' incorporates:

21830
     *  Constant: '<S363>/Constant'

21831
     */

21832
    _rtB->VectorConcatenate_i[7] = 0.0;

21833


21834
    /* UnaryMinus: '<S366>/Unary Minus' */

21835
    _rtB->VectorConcatenate_i[8] = -_rtB->sincos_o1_d[0];

21836


21837
    /* Math: '<S339>/Math Function1' */

21838
    for (i_1 = 0; i_1 < 3; i_1++) {

21839
      _rtB->MathFunction_kc[3 * i_1] = _rtB->VectorConcatenate_i[i_1];

21840
      _rtB->MathFunction_kc[1 + 3 * i_1] = _rtB->VectorConcatenate_i[i_1 + 3];

21841
      _rtB->MathFunction_kc[2 + 3 * i_1] = _rtB->VectorConcatenate_i[i_1 + 6];

21842
    }

21843


21844
    /* End of Math: '<S339>/Math Function1' */

21845


21846
    /* Gain: '<S381>/Unit Conversion' */

21847
    _rtB->UnaryMinus *= 0.017453292519943295;

21848


21849
    /* Trigonometry: '<S346>/Trigonometric Function' */

21850
    _rtB->UnaryMinus = sin(_rtB->UnaryMinus);

21851


21852
    /* Product: '<S346>/Product2' */

21853
    _rtB->UnaryMinus = _rtB->UnaryMinus * _rtB->UnaryMinus * AFCS_MODEL1_rtC(S

21854
      )->Sum1_p;

21855


21856
    /* Sum: '<S346>/Sum2' incorporates:

21857
     *  Constant: '<S346>/Constant'

21858
     */

21859
    _rtB->UnaryMinus++;

21860


21861
    /* Product: '<S346>/Product3' incorporates:

21862
     *  Constant: '<S346>/Re'

21863
     */

21864
    _rtB->UnaryMinus = 4.0680631590769E+13 / _rtB->UnaryMinus;

21865


21866
    /* Sqrt: '<S346>/sqrt' */

21867
    _rtB->UnaryMinus = sqrt(_rtB->UnaryMinus);

21868


21869
    /* UnaryMinus: '<S339>/Unary Minus' */

21870
    _rtB->UnaryMinus = -_rtB->UnaryMinus;

21871


21872
    /* SignalConversion: '<S339>/TmpSignal ConversionAtProduct1Inport2' incorporates:

21873
     *  Constant: '<S339>/Constant'

21874
     */

21875
    _rtB->TmpSignalConversionAtProductI_c[0] = 0.0;

21876
    _rtB->TmpSignalConversionAtProductI_c[1] = 0.0;

21877
    _rtB->TmpSignalConversionAtProductI_c[2] = _rtB->UnaryMinus;

21878


21879
    /* Product: '<S339>/Product1' */

21880
    for (i_1 = 0; i_1 < 3; i_1++) {

21881
      _rtB->Sum_o[i_1] = 0.0;

21882
      _rtB->Sum_o[i_1] += _rtB->MathFunction_kc[i_1] *

21883
        _rtB->TmpSignalConversionAtProductI_c[0];

21884
      _rtB->Sum_o[i_1] += _rtB->MathFunction_kc[i_1 + 3] *

21885
        _rtB->TmpSignalConversionAtProductI_c[1];

21886
      _rtB->Sum_o[i_1] += _rtB->MathFunction_kc[i_1 + 6] *

21887
        _rtB->TmpSignalConversionAtProductI_c[2];

21888
    }

21889


21890
    /* End of Product: '<S339>/Product1' */

21891


21892
    /* Gain: '<S356>/Unit Conversion' */

21893
    _rtB->sincos_o1_d[0] = 0.017453292519943295 * _rtB->Add_em;

21894
    _rtB->sincos_o1_d[1] = 0.017453292519943295 * _rtB->Switch_dn;

21895


21896
    /* Trigonometry: '<S342>/sincos' */

21897
    TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[0]);

21898
    _rtB->sincos_o1_d[0] = sin(_rtB->sincos_o1_d[0]);

21899
    _rtB->sincos_o2_j[0] = TrigonometricFunction4_o;

21900
    TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[1]);

21901
    _rtB->sincos_o1_d[1] = sin(_rtB->sincos_o1_d[1]);

21902
    _rtB->sincos_o2_j[1] = TrigonometricFunction4_o;

21903


21904
    /* Product: '<S347>/u(1)*u(4)' */

21905
    _rtB->Add_em = _rtB->sincos_o1_d[0] * _rtB->sincos_o2_j[1];

21906


21907
    /* UnaryMinus: '<S347>/Unary Minus' */

21908
    _rtB->VectorConcatenate_mc[0] = -_rtB->Add_em;

21909


21910
    /* UnaryMinus: '<S350>/Unary Minus' */

21911
    _rtB->VectorConcatenate_mc[1] = -_rtB->sincos_o1_d[1];

21912


21913
    /* Product: '<S353>/u(3)*u(4)' */

21914
    _rtB->Add_em = _rtB->sincos_o2_j[0] * _rtB->sincos_o2_j[1];

21915


21916
    /* UnaryMinus: '<S353>/Unary Minus' */

21917
    _rtB->VectorConcatenate_mc[2] = -_rtB->Add_em;

21918


21919
    /* Product: '<S348>/u(1)*u(2)' */

21920
    _rtB->Add_em = _rtB->sincos_o1_d[0] * _rtB->sincos_o1_d[1];

21921


21922
    /* UnaryMinus: '<S348>/Unary Minus' */

21923
    _rtB->VectorConcatenate_mc[3] = -_rtB->Add_em;

21924


21925
    /* SignalConversion: '<S357>/ConcatBufferAtVector ConcatenateIn5' */

21926
    _rtB->VectorConcatenate_mc[4] = _rtB->sincos_o2_j[1];

21927


21928
    /* Product: '<S354>/u(2)*u(3)' */

21929
    _rtB->Add_em = _rtB->sincos_o1_d[1] * _rtB->sincos_o2_j[0];

21930


21931
    /* UnaryMinus: '<S354>/Unary Minus' */

21932
    _rtB->VectorConcatenate_mc[5] = -_rtB->Add_em;

21933


21934
    /* SignalConversion: '<S357>/ConcatBufferAtVector ConcatenateIn7' */

21935
    _rtB->VectorConcatenate_mc[6] = _rtB->sincos_o2_j[0];

21936


21937
    /* SignalConversion: '<S357>/ConcatBufferAtVector ConcatenateIn8' incorporates:

21938
     *  Constant: '<S352>/Constant'

21939
     */

21940
    _rtB->VectorConcatenate_mc[7] = 0.0;

21941


21942
    /* UnaryMinus: '<S355>/Unary Minus' */

21943
    _rtB->VectorConcatenate_mc[8] = -_rtB->sincos_o1_d[0];

21944


21945
    /* Math: '<S339>/Math Function' */

21946
    for (i_1 = 0; i_1 < 3; i_1++) {

21947
      _rtB->MathFunction_kc[3 * i_1] = _rtB->VectorConcatenate_mc[i_1];

21948
      _rtB->MathFunction_kc[1 + 3 * i_1] = _rtB->VectorConcatenate_mc[i_1 + 3];

21949
      _rtB->MathFunction_kc[2 + 3 * i_1] = _rtB->VectorConcatenate_mc[i_1 + 6];

21950
    }

21951


21952
    /* End of Math: '<S339>/Math Function' */

21953


21954
    /* UnaryMinus: '<S339>/Unary Minus1' */

21955
    _rtB->Add_em = -_rtB->LLA0[2];

21956


21957
    /* SignalConversion: '<S339>/TmpSignal ConversionAtProductInport2' incorporates:

21958
     *  Constant: '<S339>/Constant1'

21959
     */

21960
    _rtB->TmpSignalConversionAtProductI_c[0] = 0.0;

21961
    _rtB->TmpSignalConversionAtProductI_c[1] = 0.0;

21962
    _rtB->TmpSignalConversionAtProductI_c[2] = _rtB->Add_em;

21963
    for (i_1 = 0; i_1 < 3; i_1++) {

21964
      /* Product: '<S339>/Product' */

21965
      _rtB->Divide1_g[i_1] = 0.0;

21966
      _rtB->Divide1_g[i_1] += _rtB->MathFunction_kc[i_1] *

21967
        _rtB->TmpSignalConversionAtProductI_c[0];

21968
      _rtB->Divide1_g[i_1] += _rtB->MathFunction_kc[i_1 + 3] *

21969
        _rtB->TmpSignalConversionAtProductI_c[1];

21970
      _rtB->Divide1_g[i_1] += _rtB->MathFunction_kc[i_1 + 6] *

21971
        _rtB->TmpSignalConversionAtProductI_c[2];

21972


21973
      /* Sum: '<S339>/Sum' */

21974
      _rtB->Sum_o[i_1] += _rtB->Divide1_g[i_1];

21975


21976
      /* Sum: '<S332>/Sum1' */

21977
      _rtB->Sum1_m[i_1] -= _rtB->Sum_o[i_1];

21978
    }

21979


21980
    /* Product: '<S332>/Divide1' */

21981
    for (i_1 = 0; i_1 < 3; i_1++) {

21982
      _rtB->Divide1_g[i_1] = 0.0;

21983
      _rtB->Divide1_g[i_1] += _rtB->VectorConcatenate_m[i_1] * _rtB->Sum1_m[0];

21984
      _rtB->Divide1_g[i_1] += _rtB->VectorConcatenate_m[i_1 + 3] * _rtB->Sum1_m

21985
        [1];

21986
      _rtB->Divide1_g[i_1] += _rtB->VectorConcatenate_m[i_1 + 6] * _rtB->Sum1_m

21987
        [2];

21988
    }

21989


21990
    /* End of Product: '<S332>/Divide1' */

21991


21992
    /* DataTypeConversion: '<S332>/Data Type Conversion5' */

21993
    _rtB->Xnorth_jw = (real32_T)_rtB->Divide1_g[0];

21994


21995
    /* DataTypeConversion: '<S332>/Data Type Conversion6' */

21996
    _rtB->Yeast_i = (real32_T)_rtB->Divide1_g[1];

21997


21998
    /* Sum: '<S332>/Add' */

21999
    _rtB->Add_em = _rtB->Altitude - _rtB->LLA0[2];

22000


22001
    /* DataTypeConversion: '<S332>/Data Type Conversion2' */

22002
    _rtB->h = (real32_T)_rtB->Add_em;

22003
  }

22004


22005
  if (ssIsSampleHit(S, 1, 0)) {

22006
    /* SignalConversion: '<S9>/TmpSignal ConversionAtTo WorkspaceInport1' */

22007
    _rtB->TmpSignalConversionAtToWorks_of[0] = _rtB->Xnorth_jw;

22008
    _rtB->TmpSignalConversionAtToWorks_of[1] = _rtB->Yeast_i;

22009
    _rtB->TmpSignalConversionAtToWorks_of[2] = _rtB->h;

22010


22011
    /* SignalConversion: '<S9>/TmpSignal ConversionAtTo Workspace1Inport1' */

22012
    _rtB->TmpSignalConversionAtToWorkspac[0] = _rtB->xn;

22013
    _rtB->TmpSignalConversionAtToWorkspac[1] = _rtB->ye;

22014
    _rtB->TmpSignalConversionAtToWorkspac[2] = _rtB->Alt_o;

22015


22016
    /* UnitDelay: '<S1>/Unit Delay' */

22017
    _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 367))[0];

22018


22019
    /* RelationalOperator: '<S432>/Compare' incorporates:

22020
     *  Constant: '<S432>/Constant'

22021
     */

22022
    _rtB->Compare_d = (_rtB->UnitDelay5_b >= 3);

22023


22024
    /* Delay: '<S431>/Delay' */

22025
    _rtB->RelationalOperator_l = ((boolean_T *)ssGetDWork(S, 503))[0];

22026


22027
    /* Logic: '<S431>/Logical Operator1' */

22028
    _rtB->LogicalOperator1_a = _rtB->Compare_d ^ _rtB->RelationalOperator_l;

22029


22030
    /* UnitDelay: '<S430>/Unit Delay6' */

22031
    _rtB->UnitDelay6_e = ((int8_T *)ssGetDWork(S, 368))[0];

22032


22033
    /* Chart: '<S12>/LLACalibration2' */

22034
    /* Gateway: AFCS_MODEL1/Subsystem2/LLACalibration2 */

22035
    /* During: AFCS_MODEL1/Subsystem2/LLACalibration2 */

22036
    if (((uint8_T *)ssGetDWork(S, 521))[0] == 0U) {

22037
      /* Entry: AFCS_MODEL1/Subsystem2/LLACalibration2 */

22038
      ((uint8_T *)ssGetDWork(S, 521))[0] = 1U;

22039


22040
      /* Entry Internal: AFCS_MODEL1/Subsystem2/LLACalibration2 */

22041
      /* Transition: '<S429>:146' */

22042
      ((uint8_T *)ssGetDWork(S, 522))[0] = AFCS_MODEL1_IN_AltCalibration;

22043


22044
      /* Entry 'AltCalibration': '<S429>:38' */

22045
      _rtB->TrackToPtCmd = 2;

22046
      ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_pre;

22047


22048
      /* Entry 'pre': '<S429>:144' */

22049
      ((int32_T *)ssGetDWork(S, 295))[0] = 0;

22050
      _rtB->ChangePt_Fg = 0;

22051
    } else {

22052
      /* During 'AltCalibration': '<S429>:38' */

22053
      switch (((uint8_T *)ssGetDWork(S, 523))[0]) {

22054
       case AFCS_MODEL1_IN_Alt1_o:

22055
        /* During 'Alt1': '<S429>:140' */

22056
        if (((int32_T *)ssGetDWork(S, 295))[0] == 10) {

22057
          /* Transition: '<S429>:123' */

22058
          ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_pre;

22059


22060
          /* Entry 'pre': '<S429>:144' */

22061
          ((int32_T *)ssGetDWork(S, 295))[0] = 0;

22062
          _rtB->ChangePt_Fg = 0;

22063
        } else {

22064
          tmp_1 = ((int32_T *)ssGetDWork(S, 295))[0] + 1LL;

22065
          if (tmp_1 > 2147483647LL) {

22066
            tmp_1 = 2147483647LL;

22067
          } else {

22068
            if (tmp_1 < -2147483648LL) {

22069
              tmp_1 = -2147483648LL;

22070
            }

22071
          }

22072


22073
          ((int32_T *)ssGetDWork(S, 295))[0] = (int32_T)tmp_1;

22074
        }

22075
        break;

22076


22077
       case AFCS_MODEL1_IN_Alt2:

22078
        /* During 'Alt2': '<S429>:155' */

22079
        if (((int32_T *)ssGetDWork(S, 295))[0] == 10) {

22080
          /* Transition: '<S429>:157' */

22081
          ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_pre;

22082


22083
          /* Entry 'pre': '<S429>:144' */

22084
          ((int32_T *)ssGetDWork(S, 295))[0] = 0;

22085
          _rtB->ChangePt_Fg = 0;

22086
        } else {

22087
          tmp_1 = ((int32_T *)ssGetDWork(S, 295))[0] + 1LL;

22088
          if (tmp_1 > 2147483647LL) {

22089
            tmp_1 = 2147483647LL;

22090
          } else {

22091
            if (tmp_1 < -2147483648LL) {

22092
              tmp_1 = -2147483648LL;

22093
            }

22094
          }

22095


22096
          ((int32_T *)ssGetDWork(S, 295))[0] = (int32_T)tmp_1;

22097
        }

22098
        break;

22099


22100
       default:

22101
        /* During 'pre': '<S429>:144' */

22102
        if (_rtB->LogicalOperator1_a) {

22103
          /* Transition: '<S429>:147' */

22104
          ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_Alt1_o;

22105


22106
          /* Entry 'Alt1': '<S429>:140' */

22107
          ((int32_T *)ssGetDWork(S, 295))[0] = 0;

22108
          _rtB->ChangePt_Fg = 1;

22109
          _rtB->TrackToPtCmd = 2;

22110
        } else {

22111
          if (_rtB->UnitDelay6_e == 1) {

22112
            /* Transition: '<S429>:156' */

22113
            ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_Alt2;

22114


22115
            /* Entry 'Alt2': '<S429>:155' */

22116
            ((int32_T *)ssGetDWork(S, 295))[0] = 0;

22117
            _rtB->ChangePt_Fg = 1;

22118
            _rtB->TrackToPtCmd = _rtB->TargetPiont;

22119
          }

22120
        }

22121
        break;

22122
      }

22123
    }

22124


22125
    /* End of Chart: '<S12>/LLACalibration2' */

22126


22127
    /* UnitDelay: '<S336>/Unit Delay2' */

22128
    _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 369))[0];

22129


22130
    /* Sum: '<S336>/Add1' */

22131
    _rtB->UnitDelay5_b = (int8_T)(_rtB->ChangePt_Fg - _rtB->UnitDelay5_b);

22132


22133
    /* RelationalOperator: '<S428>/Compare' incorporates:

22134
     *  Constant: '<S428>/Constant'

22135
     */

22136
    _rtB->Compare_f = (_rtB->UnitDelay5_b >= 1);

22137


22138
    /* UnitDelay: '<S430>/Unit Delay5' */

22139
    _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 370))[0];

22140


22141
    /* RelationalOperator: '<S430>/Relational Operator' */

22142
    _rtB->RelationalOperator_l = (_rtB->UnitDelay5_b != _rtB->CutPiontFG);

22143


22144
    /* DataTypeConversion: '<S430>/Data Type Conversion2' */

22145
    _rtB->DataTypeConversion2_d = (int8_T)_rtB->RelationalOperator_l;

22146


22147
    /* Memory: '<S433>/Memory1' */

22148
    _rtB->Memory1_b = ((real32_T *)ssGetDWork(S, 277))[0];

22149


22150
    /* Memory: '<S433>/Memory2' */

22151
    _rtB->Memory2 = ((real32_T *)ssGetDWork(S, 278))[0];

22152


22153
    /* Memory: '<S433>/Memory3' */

22154
    _rtB->Memory3 = ((real32_T *)ssGetDWork(S, 279))[0];

22155


22156
    /* Memory: '<S433>/Memory4' */

22157
    _rtB->Memory4 = ((real32_T *)ssGetDWork(S, 280))[0];

22158


22159
    /* Memory: '<S433>/Memory5' */

22160
    _rtB->Memory5 = ((real32_T *)ssGetDWork(S, 281))[0];

22161


22162
    /* Memory: '<S433>/Memory6' */

22163
    _rtB->Memory6 = ((real32_T *)ssGetDWork(S, 282))[0];

22164


22165
    /* Memory: '<S433>/Memory7' */

22166
    _rtB->Memory7 = ((real32_T *)ssGetDWork(S, 283))[0];

22167


22168
    /* Memory: '<S433>/Memory8' */

22169
    _rtB->Memory8 = ((real32_T *)ssGetDWork(S, 284))[0];

22170


22171
    /* Memory: '<S433>/Memory9' */

22172
    _rtB->Memory9 = ((real32_T *)ssGetDWork(S, 285))[0];

22173


22174
    /* Constant: '<S17>/Constant12' */

22175
    _rtB->mAHLow = 6000.0F;

22176
  }

22177


22178
  UNUSED_PARAMETER(tid);

22179
}

22180


22181
/* Update for root system: '<Root>' */

22182
#define MDL_UPDATE

22183


22184
static void mdlUpdate(SimStruct *S, int_T tid)

22185
{

22186
  B_AFCS_MODEL1_T *_rtB;

22187
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

22188
  if (ssIsSampleHit(S, 1, 0)) {

22189
    /* Update for Memory: '<S433>/Memory10' */

22190
    ((real32_T *)ssGetDWork(S, 270))[0] = _rtB->Memory9;

22191


22192
    /* Update for Memory: '<S438>/Memory1' */

22193
    ((real32_T *)ssGetDWork(S, 271))[0] = _rtB->decl;

22194


22195
    /* Update for Memory: '<S437>/Memory1' */

22196
    ((real32_T *)ssGetDWork(S, 272))[0] = _rtB->dacl;

22197


22198
    /* Update for Memory: '<S436>/Memory1' */

22199
    ((real32_T *)ssGetDWork(S, 273))[0] = _rtB->drcl;

22200


22201
    /* Update for Memory: '<S435>/Memory1' */

22202
    ((real32_T *)ssGetDWork(S, 274))[0] = _rtB->Sum3_kf;

22203


22204
    /* Update for Memory: '<S434>/Memory1' */

22205
    ((real32_T *)ssGetDWork(S, 275))[0] = _rtB->Sum4_h;

22206


22207
    /* Update for Memory: '<S439>/Memory1' */

22208
    ((real32_T *)ssGetDWork(S, 276))[0] = _rtB->DataTypeConversion_e;

22209
  }

22210


22211
  /* Update for Atomic SubSystem: '<S1>/Subsystem5' */

22212
  /* Update for Enabled SubSystem: '<S15>/Subsystem3' incorporates:

22213
   *  Update for EnablePort: '<S446>/Enable'

22214
   */

22215
  if (((boolean_T *)ssGetDWork(S, 614))[0]) {

22216
    if (1) {

22217
      /* Update for Integrator: '<S779>/q' */

22218
      ((int_T *)ssGetDWork(S, 344))[0] = 0;

22219


22220
      /* Update for Integrator: '<S778>/p1' */

22221
      ((int_T *)ssGetDWork(S, 345))[0] = 0;

22222


22223
      /* Update for RateLimiter: '<S465>/Rate Limiter' */

22224
      ((real_T *)ssGetDWork(S, 2))[0] = _rtB->RateLimiter[0];

22225
      ((real_T *)ssGetDWork(S, 2))[1] = _rtB->RateLimiter[1];

22226
      ((real_T *)ssGetDWork(S, 2))[2] = _rtB->RateLimiter[2];

22227
      ((real_T *)ssGetDWork(S, 3))[0] = ssGetTaskTime(S,0);

22228


22229
      /* Update for Integrator: '<S780>/ub,vb,wb' */

22230
      ((int_T *)ssGetDWork(S, 346))[0] = 0;

22231


22232
      /* Update for Backlash: '<S446>/Backlash' */

22233
      ((real_T *)ssGetDWork(S, 4))[0] = _rtB->Backlash;

22234


22235
      /* Update for Integrator: '<S470>/p,q,r ' */

22236
      ((int_T *)ssGetDWork(S, 347))[0] = 0;

22237
    }

22238


22239
    /* Update for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

22240
    AFCS_MODEL1_LMainGearGeometryForcesandMoments_Update(S,

22241
      &_rtB->LMainGearGeometryForcesandMomen);

22242


22243
    /* End of Update for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */

22244


22245
    /* Update for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

22246
    AFCS_MODEL1_RMainGearGeometryForcesandMoments_Update(S,

22247
      &_rtB->RMainGearGeometryForcesandMomen);

22248


22249
    /* End of Update for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */

22250
    if (ssIsSampleHit(S, 1, 0)) {

22251
      /* Update for UnitDelay: '<S682>/Unit Delay' incorporates:

22252
       *  Constant: '<S446>/Constant2'

22253
       */

22254
      ((int8_T *)ssGetDWork(S, 371))[0] = 1;

22255


22256
      /* Update for UnitDelay: '<S683>/Unit Delay' incorporates:

22257
       *  Constant: '<S446>/Constant2'

22258
       */

22259
      ((int8_T *)ssGetDWork(S, 372))[0] = 1;

22260


22261
      /* Update for UnitDelay: '<S619>/Unit Delay1' */

22262
      ((real_T *)ssGetDWork(S, 0))[0] = _rtB->Integrator_m;

22263
    }

22264


22265
    if (1) {

22266
      /* Update for Integrator: '<S683>/Integrator' */

22267
      ((int_T *)ssGetDWork(S, 348))[0] = 0;

22268


22269
      /* Update for Integrator: '<S682>/Integrator' */

22270
      ((int_T *)ssGetDWork(S, 349))[0] = 0;

22271
    }

22272


22273
    /* Update for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

22274
    AFCS_MODEL1_NoseGearGeometryForcesandMoments_Update(S,

22275
      &_rtB->NoseGearGeometryForcesandMoment);

22276


22277
    /* End of Update for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */

22278
    if (1) {

22279
      /* Update for Integrator: '<S778>/p' */

22280
      ((int_T *)ssGetDWork(S, 350))[0] = 0;

22281
    }

22282
  }

22283


22284
  /* End of Update for SubSystem: '<S15>/Subsystem3' */

22285
  /* End of Update for SubSystem: '<S1>/Subsystem5' */

22286
  if (ssIsSampleHit(S, 1, 0)) {

22287
    /* Update for UnitDelay: '<S24>/Unit Delay3' */

22288
    ((int8_T *)ssGetDWork(S, 352))[0] = _rtB->fmphase;

22289


22290
    /* Update for UnitDelay: '<S7>/Unit Delay' */

22291
    ((int8_T *)ssGetDWork(S, 353))[0] = _rtB->K;

22292


22293
    /* Update for UnitDelay: '<S7>/Unit Delay3' */

22294
    ((int8_T *)ssGetDWork(S, 354))[0] = _rtB->StartEmNav;

22295


22296
    /* Update for Memory: '<S9>/Memory1' */

22297
    ((boolean_T *)ssGetDWork(S, 612))[0] = _rtB->Compare_f;

22298


22299
    /* Update for Memory: '<S9>/Memory2' */

22300
    ((int8_T *)ssGetDWork(S, 504))[0] = _rtB->TrackToPtCmd;

22301


22302
    /* Update for UnitDelay: '<S9>/Unit Delay' */

22303
    ((int8_T *)ssGetDWork(S, 355))[0] = _rtB->UnitDelay1_l;

22304


22305
    /* Update for UnitDelay: '<S1>/Unit Delay1' */

22306
    ((int8_T *)ssGetDWork(S, 356))[0] = _rtB->TakeoffUL;

22307


22308
    /* Update for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */

22309
    /* Update for UnitDelay: '<S423>/Output' */

22310
    ((uint64_T *)ssGetDWork(S, 1))[0] = _rtB->FixPtSwitch;

22311


22312
    /* End of Update for SubSystem: '<S9>/Trajectory planning,tracking' */

22313


22314
    /* Update for UnitDelay: '<S31>/Unit Delay' */

22315
    ((int8_T *)ssGetDWork(S, 357))[0] = _rtB->fmpretotakeoff;

22316


22317
    /* Update for UnitDelay: '<S31>/Unit Delay1' */

22318
    ((int8_T *)ssGetDWork(S, 358))[0] = _rtB->fmTOtoClimb;

22319


22320
    /* Update for UnitDelay: '<S31>/Unit Delay2' */

22321
    ((int8_T *)ssGetDWork(S, 359))[0] = _rtB->fmClimbtoCruise;

22322


22323
    /* Update for UnitDelay: '<S31>/Unit Delay3' */

22324
    ((int8_T *)ssGetDWork(S, 360))[0] = _rtB->fmCruisetoAL;

22325


22326
    /* Update for UnitDelay: '<S31>/Unit Delay4' */

22327
    ((int8_T *)ssGetDWork(S, 361))[0] = _rtB->fmALtoLand;

22328


22329
    /* Update for UnitDelay: '<S31>/Unit Delay7' */

22330
    ((int8_T *)ssGetDWork(S, 362))[0] = _rtB->fmTOtoTaxi;

22331


22332
    /* Update for UnitDelay: '<S31>/Unit Delay5' */

22333
    ((int8_T *)ssGetDWork(S, 363))[0] = _rtB->fmLandtoTaxi;

22334


22335
    /* Update for UnitDelay: '<S32>/Unit Delay3' */

22336
    ((int8_T *)ssGetDWork(S, 364))[0] = _rtB->LiftWheelFg;

22337
  }

22338


22339
  if (1) {

22340
    /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */

22341
    {

22342
      SimStruct *rts = ssGetSFunction(S, 167);

22343
      sfcnUpdate(rts, 0);

22344
      if (ssGetErrorStatus(rts) != (NULL))

22345
        return;

22346
    }

22347


22348
    /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */

22349
    {

22350
      SimStruct *rts = ssGetSFunction(S, 168);

22351
      sfcnUpdate(rts, 0);

22352
      if (ssGetErrorStatus(rts) != (NULL))

22353
        return;

22354
    }

22355


22356
    /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */

22357
    {

22358
      SimStruct *rts = ssGetSFunction(S, 169);

22359
      sfcnUpdate(rts, 0);

22360
      if (ssGetErrorStatus(rts) != (NULL))

22361
        return;

22362
    }

22363
  }

22364


22365
  /* Update for Atomic SubSystem: '<S7>/Level' */

22366


22367
  /* Update for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */

22368
  AFCS_MODEL1_CodeReuseSubsystem_Update(S);

22369


22370
  /* End of Update for SubSystem: '<S22>/CodeReuseSubsystem' */

22371


22372
  /* End of Update for SubSystem: '<S7>/Level' */

22373
  if (ssIsSampleHit(S, 1, 0)) {

22374
    /* Update for Delay: '<S310>/Delay' */

22375
    ((boolean_T *)ssGetDWork(S, 502))[0] = _rtB->Compare_p;

22376


22377
    /* Update for UnitDelay: '<S309>/Unit Delay6' */

22378
    ((int8_T *)ssGetDWork(S, 365))[0] = _rtB->DataTypeConversion2_l;

22379


22380
    /* Update for UnitDelay: '<S309>/Unit Delay5' incorporates:

22381
     *  Constant: '<S7>/Constant'

22382
     */

22383
    ((int8_T *)ssGetDWork(S, 366))[0] = 0;

22384


22385
    /* Update for UnitDelay: '<S1>/Unit Delay' */

22386
    ((int8_T *)ssGetDWork(S, 367))[0] = _rtB->fmphase;

22387


22388
    /* Update for Delay: '<S431>/Delay' */

22389
    ((boolean_T *)ssGetDWork(S, 503))[0] = _rtB->Compare_d;

22390


22391
    /* Update for UnitDelay: '<S430>/Unit Delay6' */

22392
    ((int8_T *)ssGetDWork(S, 368))[0] = _rtB->DataTypeConversion2_d;

22393


22394
    /* Update for UnitDelay: '<S336>/Unit Delay2' */

22395
    ((int8_T *)ssGetDWork(S, 369))[0] = _rtB->ChangePt_Fg;

22396


22397
    /* Update for UnitDelay: '<S430>/Unit Delay5' */

22398
    ((int8_T *)ssGetDWork(S, 370))[0] = _rtB->CutPiontFG;

22399


22400
    /* Update for Memory: '<S433>/Memory1' */

22401
    ((real32_T *)ssGetDWork(S, 277))[0] = _rtB->thrust;

22402


22403
    /* Update for Memory: '<S433>/Memory2' */

22404
    ((real32_T *)ssGetDWork(S, 278))[0] = _rtB->Memory1_b;

22405


22406
    /* Update for Memory: '<S433>/Memory3' */

22407
    ((real32_T *)ssGetDWork(S, 279))[0] = _rtB->Memory2;

22408


22409
    /* Update for Memory: '<S433>/Memory4' */

22410
    ((real32_T *)ssGetDWork(S, 280))[0] = _rtB->Memory3;

22411


22412
    /* Update for Memory: '<S433>/Memory5' */

22413
    ((real32_T *)ssGetDWork(S, 281))[0] = _rtB->Memory4;

22414


22415
    /* Update for Memory: '<S433>/Memory6' */

22416
    ((real32_T *)ssGetDWork(S, 282))[0] = _rtB->Memory5;

22417


22418
    /* Update for Memory: '<S433>/Memory7' */

22419
    ((real32_T *)ssGetDWork(S, 283))[0] = _rtB->Memory6;

22420


22421
    /* Update for Memory: '<S433>/Memory8' */

22422
    ((real32_T *)ssGetDWork(S, 284))[0] = _rtB->Memory7;

22423


22424
    /* Update for Memory: '<S433>/Memory9' */

22425
    ((real32_T *)ssGetDWork(S, 285))[0] = _rtB->Memory8;

22426
  }

22427


22428
  UNUSED_PARAMETER(tid);

22429
}

22430


22431
/* Derivatives for root system: '<Root>' */

22432
#define MDL_DERIVATIVES

22433


22434
static void mdlDerivatives(SimStruct *S)

22435
{

22436
  boolean_T lsat;

22437
  boolean_T usat;

22438
  B_AFCS_MODEL1_T *_rtB;

22439
  X_AFCS_MODEL1_T *_rtX;

22440
  XDot_AFCS_MODEL1_T *_rtXdot;

22441
  _rtXdot = ((XDot_AFCS_MODEL1_T *) ssGetdX(S));

22442
  _rtX = ((X_AFCS_MODEL1_T *) ssGetContStates(S));

22443
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

22444


22445
  /* Derivatives for Atomic SubSystem: '<S1>/Subsystem5' */

22446
  /* Derivatives for Enabled SubSystem: '<S15>/Subsystem3' */

22447
  if (((boolean_T *)ssGetDWork(S, 614))[0]) {

22448
    /* Derivatives for Integrator: '<S472>/Servo rate' */

22449
    _rtXdot->Servorate_CSTATE = _rtB->Sum2;

22450


22451
    /* Derivatives for Integrator: '<S472>/Limited Deflection' */

22452
    lsat = (_rtX->LimitedDeflection_CSTATE <= -30.0);

22453
    usat = (_rtX->LimitedDeflection_CSTATE >= 30.0);

22454
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate > 0.0)) || (usat &&

22455
         (_rtB->Servorate < 0.0))) {

22456
      _rtXdot->LimitedDeflection_CSTATE = _rtB->Servorate;

22457
    } else {

22458
      /* in saturation */

22459
      _rtXdot->LimitedDeflection_CSTATE = 0.0;

22460
    }

22461


22462
    /* End of Derivatives for Integrator: '<S472>/Limited Deflection' */

22463


22464
    /* Derivatives for Integrator: '<S473>/Servo rate' */

22465
    _rtXdot->Servorate_CSTATE_n = _rtB->Sum2_o;

22466


22467
    /* Derivatives for Integrator: '<S473>/Limited Deflection' */

22468
    lsat = (_rtX->LimitedDeflection_CSTATE_i <= -30.0);

22469
    usat = (_rtX->LimitedDeflection_CSTATE_i >= 30.0);

22470
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_l > 0.0)) || (usat &&

22471
         (_rtB->Servorate_l < 0.0))) {

22472
      _rtXdot->LimitedDeflection_CSTATE_i = _rtB->Servorate_l;

22473
    } else {

22474
      /* in saturation */

22475
      _rtXdot->LimitedDeflection_CSTATE_i = 0.0;

22476
    }

22477


22478
    /* End of Derivatives for Integrator: '<S473>/Limited Deflection' */

22479


22480
    /* Derivatives for Integrator: '<S474>/Servo rate' */

22481
    _rtXdot->Servorate_CSTATE_m = _rtB->Sum2_f;

22482


22483
    /* Derivatives for Integrator: '<S474>/Limited Deflection' */

22484
    lsat = (_rtX->LimitedDeflection_CSTATE_m <= -30.0);

22485
    usat = (_rtX->LimitedDeflection_CSTATE_m >= 30.0);

22486
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_lu > 0.0)) || (usat &&

22487
         (_rtB->Servorate_lu < 0.0))) {

22488
      _rtXdot->LimitedDeflection_CSTATE_m = _rtB->Servorate_lu;

22489
    } else {

22490
      /* in saturation */

22491
      _rtXdot->LimitedDeflection_CSTATE_m = 0.0;

22492
    }

22493


22494
    /* End of Derivatives for Integrator: '<S474>/Limited Deflection' */

22495


22496
    /* Derivatives for Integrator: '<S475>/Servo rate' */

22497
    _rtXdot->Servorate_CSTATE_mi = _rtB->Sum2_i;

22498


22499
    /* Derivatives for Integrator: '<S475>/Limited Deflection' */

22500
    lsat = (_rtX->LimitedDeflection_CSTATE_f <= -30.0);

22501
    usat = (_rtX->LimitedDeflection_CSTATE_f >= 30.0);

22502
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_g > 0.0)) || (usat &&

22503
         (_rtB->Servorate_g < 0.0))) {

22504
      _rtXdot->LimitedDeflection_CSTATE_f = _rtB->Servorate_g;

22505
    } else {

22506
      /* in saturation */

22507
      _rtXdot->LimitedDeflection_CSTATE_f = 0.0;

22508
    }

22509


22510
    /* End of Derivatives for Integrator: '<S475>/Limited Deflection' */

22511


22512
    /* Derivatives for Integrator: '<S476>/Servo rate' */

22513
    _rtXdot->Servorate_CSTATE_f = _rtB->Sum2_p;

22514


22515
    /* Derivatives for Integrator: '<S476>/Limited Deflection' */

22516
    lsat = (_rtX->LimitedDeflection_CSTATE_g <= -30.0);

22517
    usat = (_rtX->LimitedDeflection_CSTATE_g >= 30.0);

22518
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_m > 0.0)) || (usat &&

22519
         (_rtB->Servorate_m < 0.0))) {

22520
      _rtXdot->LimitedDeflection_CSTATE_g = _rtB->Servorate_m;

22521
    } else {

22522
      /* in saturation */

22523
      _rtXdot->LimitedDeflection_CSTATE_g = 0.0;

22524
    }

22525


22526
    /* End of Derivatives for Integrator: '<S476>/Limited Deflection' */

22527


22528
    /* Derivatives for Integrator: '<S477>/Servo rate' */

22529
    _rtXdot->Servorate_CSTATE_k = _rtB->Sum2_m;

22530


22531
    /* Derivatives for Integrator: '<S477>/Limited Deflection' */

22532
    lsat = (_rtX->LimitedDeflection_CSTATE_l <= -30.0);

22533
    usat = (_rtX->LimitedDeflection_CSTATE_l >= 30.0);

22534
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_ga > 0.0)) || (usat &&

22535
         (_rtB->Servorate_ga < 0.0))) {

22536
      _rtXdot->LimitedDeflection_CSTATE_l = _rtB->Servorate_ga;

22537
    } else {

22538
      /* in saturation */

22539
      _rtXdot->LimitedDeflection_CSTATE_l = 0.0;

22540
    }

22541


22542
    /* End of Derivatives for Integrator: '<S477>/Limited Deflection' */

22543


22544
    /* Derivatives for Integrator: '<S478>/Servo rate' */

22545
    _rtXdot->Servorate_CSTATE_h = _rtB->Sum2_k;

22546


22547
    /* Derivatives for Integrator: '<S478>/Limited Deflection' */

22548
    lsat = (_rtX->LimitedDeflection_CSTATE_j <= -30.0);

22549
    usat = (_rtX->LimitedDeflection_CSTATE_j >= 30.0);

22550
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_d > 0.0)) || (usat &&

22551
         (_rtB->Servorate_d < 0.0))) {

22552
      _rtXdot->LimitedDeflection_CSTATE_j = _rtB->Servorate_d;

22553
    } else {

22554
      /* in saturation */

22555
      _rtXdot->LimitedDeflection_CSTATE_j = 0.0;

22556
    }

22557


22558
    /* End of Derivatives for Integrator: '<S478>/Limited Deflection' */

22559


22560
    /* Derivatives for Integrator: '<S479>/Servo rate' */

22561
    _rtXdot->Servorate_CSTATE_kx = _rtB->Sum2_mm;

22562


22563
    /* Derivatives for Integrator: '<S479>/Limited Deflection' */

22564
    lsat = (_rtX->LimitedDeflection_CSTATE_la <= -30.0);

22565
    usat = (_rtX->LimitedDeflection_CSTATE_la >= 30.0);

22566
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_h > 0.0)) || (usat &&

22567
         (_rtB->Servorate_h < 0.0))) {

22568
      _rtXdot->LimitedDeflection_CSTATE_la = _rtB->Servorate_h;

22569
    } else {

22570
      /* in saturation */

22571
      _rtXdot->LimitedDeflection_CSTATE_la = 0.0;

22572
    }

22573


22574
    /* End of Derivatives for Integrator: '<S479>/Limited Deflection' */

22575


22576
    /* Derivatives for TransferFcn: '<S462>/TransferFcn1' */

22577
    _rtXdot->TransferFcn1_CSTATE = 0.0;

22578
    _rtXdot->TransferFcn1_CSTATE += -7.1710290426676231 *

22579
      _rtX->TransferFcn1_CSTATE;

22580
    _rtXdot->TransferFcn1_CSTATE += _rtB->DataTypeConversion4[0];

22581


22582
    /* Derivatives for Integrator: '<S779>/q' */

22583
    _rtXdot->q_CSTATE[0] = _rtB->q0dot;

22584
    _rtXdot->q_CSTATE[1] = _rtB->q1dot;

22585
    _rtXdot->q_CSTATE[2] = _rtB->q2dot;

22586
    _rtXdot->q_CSTATE[3] = _rtB->q3dot;

22587


22588
    /* Derivatives for Integrator: '<S778>/p1' */

22589
    _rtXdot->p1_CSTATE[0] = _rtB->Product5[0];

22590


22591
    /* Derivatives for Integrator: '<S780>/ub,vb,wb' */

22592
    _rtXdot->ubvbwb_CSTATE[0] = _rtB->Sum_e[0];

22593


22594
    /* Derivatives for Integrator: '<S470>/p,q,r ' */

22595
    _rtXdot->pqr_CSTATE[0] = _rtB->Reshape[0];

22596


22597
    /* Derivatives for Integrator: '<S778>/p1' */

22598
    _rtXdot->p1_CSTATE[1] = _rtB->Product5[1];

22599


22600
    /* Derivatives for Integrator: '<S780>/ub,vb,wb' */

22601
    _rtXdot->ubvbwb_CSTATE[1] = _rtB->Sum_e[1];

22602


22603
    /* Derivatives for Integrator: '<S470>/p,q,r ' */

22604
    _rtXdot->pqr_CSTATE[1] = _rtB->Reshape[1];

22605


22606
    /* Derivatives for Integrator: '<S778>/p1' */

22607
    _rtXdot->p1_CSTATE[2] = _rtB->Product5[2];

22608


22609
    /* Derivatives for Integrator: '<S780>/ub,vb,wb' */

22610
    _rtXdot->ubvbwb_CSTATE[2] = _rtB->Sum_e[2];

22611


22612
    /* Derivatives for Integrator: '<S470>/p,q,r ' */

22613
    _rtXdot->pqr_CSTATE[2] = _rtB->Reshape[2];

22614


22615
    /* Derivatives for Integrator: '<S683>/Integrator' */

22616
    lsat = (_rtX->Integrator_CSTATE <= -8.0);

22617
    usat = (_rtX->Integrator_CSTATE >= 8.0);

22618
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Saturation1 > 0.0)) || (usat &&

22619
         (_rtB->Saturation1 < 0.0))) {

22620
      _rtXdot->Integrator_CSTATE = _rtB->Saturation1;

22621
    } else {

22622
      /* in saturation */

22623
      _rtXdot->Integrator_CSTATE = 0.0;

22624
    }

22625


22626
    /* End of Derivatives for Integrator: '<S683>/Integrator' */

22627


22628
    /* Derivatives for Integrator: '<S682>/Integrator' */

22629
    lsat = (_rtX->Integrator_CSTATE_l <= -30.0);

22630
    usat = (_rtX->Integrator_CSTATE_l >= 30.0);

22631
    if (((!lsat) && (!usat)) || (lsat && (_rtB->Saturation > 0.0)) || (usat &&

22632
         (_rtB->Saturation < 0.0))) {

22633
      _rtXdot->Integrator_CSTATE_l = _rtB->Saturation;

22634
    } else {

22635
      /* in saturation */

22636
      _rtXdot->Integrator_CSTATE_l = 0.0;

22637
    }

22638


22639
    /* End of Derivatives for Integrator: '<S682>/Integrator' */

22640


22641
    /* Derivatives for TransferFcn: '<S683>/Transfer Fcn2' */

22642
    _rtXdot->TransferFcn2_CSTATE[0] = 0.0;

22643
    _rtXdot->TransferFcn2_CSTATE[0] += -10.0 * _rtX->TransferFcn2_CSTATE[0];

22644
    _rtXdot->TransferFcn2_CSTATE[1] = 0.0;

22645
    _rtXdot->TransferFcn2_CSTATE[0] += -16.0 * _rtX->TransferFcn2_CSTATE[1];

22646
    _rtXdot->TransferFcn2_CSTATE[1] += _rtX->TransferFcn2_CSTATE[0];

22647


22648
    /* Derivatives for Integrator: '<S781>/Integrator' incorporates:

22649
     *  Constant: '<S781>/Constant1'

22650
     */

22651
    _rtXdot->Integrator_CSTATE_a = 7.292115E-5;

22652


22653
    /* Derivatives for Integrator: '<S778>/p' */

22654
    _rtXdot->p_CSTATE[0] = _rtB->Sum2_e[0];

22655
    _rtXdot->p_CSTATE[1] = _rtB->Sum2_e[1];

22656
    _rtXdot->p_CSTATE[2] = _rtB->Sum2_e[2];

22657


22658
    /* Derivatives for TransferFcn: '<S446>/Transfer Fcn' */

22659
    _rtXdot->TransferFcn_CSTATE = 0.0;

22660
    _rtXdot->TransferFcn_CSTATE += -50.0 * _rtX->TransferFcn_CSTATE;

22661
    _rtXdot->TransferFcn_CSTATE += _rtB->Switch_h;

22662
  } else {

22663
    {

22664
      real_T *dx;

22665
      int_T i;

22666
      dx = &(((XDot_AFCS_MODEL1_T *) ssGetdX(S))->Servorate_CSTATE);

22667
      for (i=0; i < 39; i++) {

22668
        dx[i] = 0.0;

22669
      }

22670
    }

22671
  }

22672


22673
  /* End of Derivatives for SubSystem: '<S15>/Subsystem3' */

22674
  /* End of Derivatives for SubSystem: '<S1>/Subsystem5' */

22675
}

22676


22677
/* Termination for root system: '<Root>' */

22678
static void mdlTerminate(SimStruct *S)

22679
{

22680
  B_AFCS_MODEL1_T *_rtB;

22681
  _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));

22682


22683
  /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */

22684
  {

22685
    SimStruct *rts = ssGetSFunction(S, 167);

22686
    sfcnTerminate(rts);

22687
  }

22688


22689
  /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */

22690
  {

22691
    SimStruct *rts = ssGetSFunction(S, 168);

22692
    sfcnTerminate(rts);

22693
  }

22694


22695
  /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */

22696
  {

22697
    SimStruct *rts = ssGetSFunction(S, 169);

22698
    sfcnTerminate(rts);

22699
  }

22700


22701
  /* Terminate for Atomic SubSystem: '<S7>/Level' */

22702


22703
  /* Terminate for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */

22704
  AFCS_MODEL1_CodeReuseSubsystem_Term(S);

22705


22706
  /* End of Terminate for SubSystem: '<S22>/CodeReuseSubsystem' */

22707


22708
  /* End of Terminate for SubSystem: '<S7>/Level' */

22709
#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)

22710


22711
  /* child S-function code */

22712
  if (_ssGetSFunctions(S) ) {

22713
    /* RTW Generated Level2 S-Function Block: '<S174>/Generated S-Function' (LagFilter_sf) */

22714
    {

22715
      SimStruct *rts = ssGetSFunction(S, 0);

22716
      if (rts != (NULL) ) {

22717
        rt_FREE(ssGetSampleTimePtr(rts));

22718
        rt_FREE(ssGetOffsetTimePtr(rts));

22719
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

22720


22721
        /* Destroy model methods 3 */

22722
        {

22723
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

22724
          rt_FREE(methods3);

22725
        }

22726


22727
        /* Destroy model methods 2 */

22728
        {

22729
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

22730
          rt_FREE(methods2);

22731
        }

22732


22733
        /* Destroy state auxilliary information */

22734
        {

22735
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

22736
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

22737
          rt_FREE(periodicStatesInfo);

22738
          rt_FREE(statesInfo2);

22739
        }

22740


22741
        {

22742
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

22743
          rt_FREE(ptr);

22744
        }

22745


22746
        {

22747
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

22748
          rt_FREE(ptr);

22749
        }

22750


22751
        {

22752
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

22753
          rt_FREE(ptr);

22754
        }

22755


22756
        {

22757
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

22758
          rt_FREE(ptr);

22759
        }

22760


22761
        {

22762
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

22763
          rt_FREE(ptr);

22764
        }

22765


22766
        {

22767
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

22768
          rt_FREE(ptr);

22769
        }

22770


22771
        rt_FREE(ssGetPortInfoForInputs(rts));

22772
        rt_FREE(ssGetPortInfoForOutputs(rts));

22773
        rt_FREE(ssGetSFcnDWork(rts));

22774
      }

22775
    }

22776


22777
    /* RTW Generated Level2 S-Function Block: '<S186>/Generated S-Function' (SaturationLimiter_sf) */

22778
    {

22779
      SimStruct *rts = ssGetSFunction(S, 1);

22780
      if (rts != (NULL) ) {

22781
        rt_FREE(ssGetSampleTimePtr(rts));

22782
        rt_FREE(ssGetOffsetTimePtr(rts));

22783
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

22784


22785
        /* Destroy model methods 3 */

22786
        {

22787
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

22788
          rt_FREE(methods3);

22789
        }

22790


22791
        /* Destroy model methods 2 */

22792
        {

22793
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

22794
          rt_FREE(methods2);

22795
        }

22796


22797
        /* Destroy state auxilliary information */

22798
        {

22799
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

22800
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

22801
          rt_FREE(periodicStatesInfo);

22802
          rt_FREE(statesInfo2);

22803
        }

22804


22805
        {

22806
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

22807
          rt_FREE(ptr);

22808
        }

22809


22810
        {

22811
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

22812
          rt_FREE(ptr);

22813
        }

22814


22815
        {

22816
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

22817
          rt_FREE(ptr);

22818
        }

22819


22820
        rt_FREE(ssGetPortInfoForInputs(rts));

22821
        rt_FREE(ssGetPortInfoForOutputs(rts));

22822
      }

22823
    }

22824


22825
    /* RTW Generated Level2 S-Function Block: '<S188>/Generated S-Function' (Gain_sf) */

22826
    {

22827
      SimStruct *rts = ssGetSFunction(S, 2);

22828
      if (rts != (NULL) ) {

22829
        rt_FREE(ssGetSampleTimePtr(rts));

22830
        rt_FREE(ssGetOffsetTimePtr(rts));

22831
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

22832


22833
        /* Destroy model methods 3 */

22834
        {

22835
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

22836
          rt_FREE(methods3);

22837
        }

22838


22839
        /* Destroy model methods 2 */

22840
        {

22841
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

22842
          rt_FREE(methods2);

22843
        }

22844


22845
        /* Destroy state auxilliary information */

22846
        {

22847
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

22848
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

22849
          rt_FREE(periodicStatesInfo);

22850
          rt_FREE(statesInfo2);

22851
        }

22852


22853
        {

22854
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

22855
          rt_FREE(ptr);

22856
        }

22857


22858
        {

22859
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

22860
          rt_FREE(ptr);

22861
        }

22862


22863
        rt_FREE(ssGetPortInfoForInputs(rts));

22864
        rt_FREE(ssGetPortInfoForOutputs(rts));

22865
      }

22866
    }

22867


22868
    /* RTW Generated Level2 S-Function Block: '<S185>/Generated S-Function' (LeadLagFilter_sf) */

22869
    {

22870
      SimStruct *rts = ssGetSFunction(S, 3);

22871
      if (rts != (NULL) ) {

22872
        rt_FREE(ssGetSampleTimePtr(rts));

22873
        rt_FREE(ssGetOffsetTimePtr(rts));

22874
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

22875


22876
        /* Destroy model methods 3 */

22877
        {

22878
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

22879
          rt_FREE(methods3);

22880
        }

22881


22882
        /* Destroy model methods 2 */

22883
        {

22884
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

22885
          rt_FREE(methods2);

22886
        }

22887


22888
        /* Destroy state auxilliary information */

22889
        {

22890
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

22891
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

22892
          rt_FREE(periodicStatesInfo);

22893
          rt_FREE(statesInfo2);

22894
        }

22895


22896
        {

22897
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

22898
          rt_FREE(ptr);

22899
        }

22900


22901
        {

22902
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

22903
          rt_FREE(ptr);

22904
        }

22905


22906
        {

22907
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

22908
          rt_FREE(ptr);

22909
        }

22910


22911
        {

22912
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

22913
          rt_FREE(ptr);

22914
        }

22915


22916
        {

22917
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

22918
          rt_FREE(ptr);

22919
        }

22920


22921
        {

22922
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

22923
          rt_FREE(ptr);

22924
        }

22925


22926
        {

22927
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

22928
          rt_FREE(ptr);

22929
        }

22930


22931
        rt_FREE(ssGetPortInfoForInputs(rts));

22932
        rt_FREE(ssGetPortInfoForOutputs(rts));

22933
        rt_FREE(ssGetSFcnDWork(rts));

22934
      }

22935
    }

22936


22937
    /* RTW Generated Level2 S-Function Block: '<S187>/Generated S-Function' (SaturationLimiter_sf) */

22938
    {

22939
      SimStruct *rts = ssGetSFunction(S, 4);

22940
      if (rts != (NULL) ) {

22941
        rt_FREE(ssGetSampleTimePtr(rts));

22942
        rt_FREE(ssGetOffsetTimePtr(rts));

22943
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

22944


22945
        /* Destroy model methods 3 */

22946
        {

22947
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

22948
          rt_FREE(methods3);

22949
        }

22950


22951
        /* Destroy model methods 2 */

22952
        {

22953
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

22954
          rt_FREE(methods2);

22955
        }

22956


22957
        /* Destroy state auxilliary information */

22958
        {

22959
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

22960
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

22961
          rt_FREE(periodicStatesInfo);

22962
          rt_FREE(statesInfo2);

22963
        }

22964


22965
        {

22966
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

22967
          rt_FREE(ptr);

22968
        }

22969


22970
        {

22971
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

22972
          rt_FREE(ptr);

22973
        }

22974


22975
        {

22976
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

22977
          rt_FREE(ptr);

22978
        }

22979


22980
        rt_FREE(ssGetPortInfoForInputs(rts));

22981
        rt_FREE(ssGetPortInfoForOutputs(rts));

22982
      }

22983
    }

22984


22985
    /* RTW Generated Level2 S-Function Block: '<S189>/Generated S-Function' (Gain_sf) */

22986
    {

22987
      SimStruct *rts = ssGetSFunction(S, 5);

22988
      if (rts != (NULL) ) {

22989
        rt_FREE(ssGetSampleTimePtr(rts));

22990
        rt_FREE(ssGetOffsetTimePtr(rts));

22991
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

22992


22993
        /* Destroy model methods 3 */

22994
        {

22995
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

22996
          rt_FREE(methods3);

22997
        }

22998


22999
        /* Destroy model methods 2 */

23000
        {

23001
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23002
          rt_FREE(methods2);

23003
        }

23004


23005
        /* Destroy state auxilliary information */

23006
        {

23007
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23008
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23009
          rt_FREE(periodicStatesInfo);

23010
          rt_FREE(statesInfo2);

23011
        }

23012


23013
        {

23014
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23015
          rt_FREE(ptr);

23016
        }

23017


23018
        {

23019
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23020
          rt_FREE(ptr);

23021
        }

23022


23023
        rt_FREE(ssGetPortInfoForInputs(rts));

23024
        rt_FREE(ssGetPortInfoForOutputs(rts));

23025
      }

23026
    }

23027


23028
    /* RTW Generated Level2 S-Function Block: '<S184>/Generated S-Function' (IntegratorLimited_sf) */

23029
    {

23030
      SimStruct *rts = ssGetSFunction(S, 6);

23031
      if (rts != (NULL) ) {

23032
        rt_FREE(ssGetSampleTimePtr(rts));

23033
        rt_FREE(ssGetOffsetTimePtr(rts));

23034
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23035


23036
        /* Destroy model methods 3 */

23037
        {

23038
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23039
          rt_FREE(methods3);

23040
        }

23041


23042
        /* Destroy model methods 2 */

23043
        {

23044
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23045
          rt_FREE(methods2);

23046
        }

23047


23048
        /* Destroy state auxilliary information */

23049
        {

23050
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23051
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23052
          rt_FREE(periodicStatesInfo);

23053
          rt_FREE(statesInfo2);

23054
        }

23055


23056
        {

23057
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23058
          rt_FREE(ptr);

23059
        }

23060


23061
        {

23062
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23063
          rt_FREE(ptr);

23064
        }

23065


23066
        {

23067
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23068
          rt_FREE(ptr);

23069
        }

23070


23071
        {

23072
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

23073
          rt_FREE(ptr);

23074
        }

23075


23076
        {

23077
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

23078
          rt_FREE(ptr);

23079
        }

23080


23081
        {

23082
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

23083
          rt_FREE(ptr);

23084
        }

23085


23086
        {

23087
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

23088
          rt_FREE(ptr);

23089
        }

23090


23091
        rt_FREE(ssGetPortInfoForInputs(rts));

23092
        rt_FREE(ssGetPortInfoForOutputs(rts));

23093
        rt_FREE(ssGetSFcnDWork(rts));

23094
      }

23095
    }

23096


23097
    /* RTW Generated Level2 S-Function Block: '<S183>/Generated S-Function' (LagFilter_sf) */

23098
    {

23099
      SimStruct *rts = ssGetSFunction(S, 7);

23100
      if (rts != (NULL) ) {

23101
        rt_FREE(ssGetSampleTimePtr(rts));

23102
        rt_FREE(ssGetOffsetTimePtr(rts));

23103
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23104


23105
        /* Destroy model methods 3 */

23106
        {

23107
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23108
          rt_FREE(methods3);

23109
        }

23110


23111
        /* Destroy model methods 2 */

23112
        {

23113
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23114
          rt_FREE(methods2);

23115
        }

23116


23117
        /* Destroy state auxilliary information */

23118
        {

23119
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23120
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23121
          rt_FREE(periodicStatesInfo);

23122
          rt_FREE(statesInfo2);

23123
        }

23124


23125
        {

23126
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23127
          rt_FREE(ptr);

23128
        }

23129


23130
        {

23131
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23132
          rt_FREE(ptr);

23133
        }

23134


23135
        {

23136
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23137
          rt_FREE(ptr);

23138
        }

23139


23140
        {

23141
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

23142
          rt_FREE(ptr);

23143
        }

23144


23145
        {

23146
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

23147
          rt_FREE(ptr);

23148
        }

23149


23150
        {

23151
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

23152
          rt_FREE(ptr);

23153
        }

23154


23155
        rt_FREE(ssGetPortInfoForInputs(rts));

23156
        rt_FREE(ssGetPortInfoForOutputs(rts));

23157
        rt_FREE(ssGetSFcnDWork(rts));

23158
      }

23159
    }

23160


23161
    /* RTW Generated Level2 S-Function Block: '<S179>/Generated S-Function' (SaturationLimiter_sf) */

23162
    {

23163
      SimStruct *rts = ssGetSFunction(S, 8);

23164
      if (rts != (NULL) ) {

23165
        rt_FREE(ssGetSampleTimePtr(rts));

23166
        rt_FREE(ssGetOffsetTimePtr(rts));

23167
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23168


23169
        /* Destroy model methods 3 */

23170
        {

23171
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23172
          rt_FREE(methods3);

23173
        }

23174


23175
        /* Destroy model methods 2 */

23176
        {

23177
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23178
          rt_FREE(methods2);

23179
        }

23180


23181
        /* Destroy state auxilliary information */

23182
        {

23183
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23184
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23185
          rt_FREE(periodicStatesInfo);

23186
          rt_FREE(statesInfo2);

23187
        }

23188


23189
        {

23190
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23191
          rt_FREE(ptr);

23192
        }

23193


23194
        {

23195
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23196
          rt_FREE(ptr);

23197
        }

23198


23199
        {

23200
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23201
          rt_FREE(ptr);

23202
        }

23203


23204
        rt_FREE(ssGetPortInfoForInputs(rts));

23205
        rt_FREE(ssGetPortInfoForOutputs(rts));

23206
      }

23207
    }

23208


23209
    /* RTW Generated Level2 S-Function Block: '<S181>/Generated S-Function' (Gain_sf) */

23210
    {

23211
      SimStruct *rts = ssGetSFunction(S, 9);

23212
      if (rts != (NULL) ) {

23213
        rt_FREE(ssGetSampleTimePtr(rts));

23214
        rt_FREE(ssGetOffsetTimePtr(rts));

23215
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23216


23217
        /* Destroy model methods 3 */

23218
        {

23219
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23220
          rt_FREE(methods3);

23221
        }

23222


23223
        /* Destroy model methods 2 */

23224
        {

23225
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23226
          rt_FREE(methods2);

23227
        }

23228


23229
        /* Destroy state auxilliary information */

23230
        {

23231
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23232
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23233
          rt_FREE(periodicStatesInfo);

23234
          rt_FREE(statesInfo2);

23235
        }

23236


23237
        {

23238
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23239
          rt_FREE(ptr);

23240
        }

23241


23242
        {

23243
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23244
          rt_FREE(ptr);

23245
        }

23246


23247
        rt_FREE(ssGetPortInfoForInputs(rts));

23248
        rt_FREE(ssGetPortInfoForOutputs(rts));

23249
      }

23250
    }

23251


23252
    /* RTW Generated Level2 S-Function Block: '<S178>/Generated S-Function' (LeadLagFilter_sf) */

23253
    {

23254
      SimStruct *rts = ssGetSFunction(S, 10);

23255
      if (rts != (NULL) ) {

23256
        rt_FREE(ssGetSampleTimePtr(rts));

23257
        rt_FREE(ssGetOffsetTimePtr(rts));

23258
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23259


23260
        /* Destroy model methods 3 */

23261
        {

23262
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23263
          rt_FREE(methods3);

23264
        }

23265


23266
        /* Destroy model methods 2 */

23267
        {

23268
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23269
          rt_FREE(methods2);

23270
        }

23271


23272
        /* Destroy state auxilliary information */

23273
        {

23274
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23275
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23276
          rt_FREE(periodicStatesInfo);

23277
          rt_FREE(statesInfo2);

23278
        }

23279


23280
        {

23281
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23282
          rt_FREE(ptr);

23283
        }

23284


23285
        {

23286
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23287
          rt_FREE(ptr);

23288
        }

23289


23290
        {

23291
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23292
          rt_FREE(ptr);

23293
        }

23294


23295
        {

23296
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

23297
          rt_FREE(ptr);

23298
        }

23299


23300
        {

23301
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

23302
          rt_FREE(ptr);

23303
        }

23304


23305
        {

23306
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

23307
          rt_FREE(ptr);

23308
        }

23309


23310
        {

23311
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

23312
          rt_FREE(ptr);

23313
        }

23314


23315
        rt_FREE(ssGetPortInfoForInputs(rts));

23316
        rt_FREE(ssGetPortInfoForOutputs(rts));

23317
        rt_FREE(ssGetSFcnDWork(rts));

23318
      }

23319
    }

23320


23321
    /* RTW Generated Level2 S-Function Block: '<S180>/Generated S-Function' (SaturationLimiter_sf) */

23322
    {

23323
      SimStruct *rts = ssGetSFunction(S, 11);

23324
      if (rts != (NULL) ) {

23325
        rt_FREE(ssGetSampleTimePtr(rts));

23326
        rt_FREE(ssGetOffsetTimePtr(rts));

23327
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23328


23329
        /* Destroy model methods 3 */

23330
        {

23331
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23332
          rt_FREE(methods3);

23333
        }

23334


23335
        /* Destroy model methods 2 */

23336
        {

23337
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23338
          rt_FREE(methods2);

23339
        }

23340


23341
        /* Destroy state auxilliary information */

23342
        {

23343
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23344
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23345
          rt_FREE(periodicStatesInfo);

23346
          rt_FREE(statesInfo2);

23347
        }

23348


23349
        {

23350
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23351
          rt_FREE(ptr);

23352
        }

23353


23354
        {

23355
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23356
          rt_FREE(ptr);

23357
        }

23358


23359
        {

23360
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23361
          rt_FREE(ptr);

23362
        }

23363


23364
        rt_FREE(ssGetPortInfoForInputs(rts));

23365
        rt_FREE(ssGetPortInfoForOutputs(rts));

23366
      }

23367
    }

23368


23369
    /* RTW Generated Level2 S-Function Block: '<S182>/Generated S-Function' (Gain_sf) */

23370
    {

23371
      SimStruct *rts = ssGetSFunction(S, 12);

23372
      if (rts != (NULL) ) {

23373
        rt_FREE(ssGetSampleTimePtr(rts));

23374
        rt_FREE(ssGetOffsetTimePtr(rts));

23375
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23376


23377
        /* Destroy model methods 3 */

23378
        {

23379
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23380
          rt_FREE(methods3);

23381
        }

23382


23383
        /* Destroy model methods 2 */

23384
        {

23385
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23386
          rt_FREE(methods2);

23387
        }

23388


23389
        /* Destroy state auxilliary information */

23390
        {

23391
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23392
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23393
          rt_FREE(periodicStatesInfo);

23394
          rt_FREE(statesInfo2);

23395
        }

23396


23397
        {

23398
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23399
          rt_FREE(ptr);

23400
        }

23401


23402
        {

23403
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23404
          rt_FREE(ptr);

23405
        }

23406


23407
        rt_FREE(ssGetPortInfoForInputs(rts));

23408
        rt_FREE(ssGetPortInfoForOutputs(rts));

23409
      }

23410
    }

23411


23412
    /* RTW Generated Level2 S-Function Block: '<S177>/Generated S-Function' (IntegratorLimited_sf) */

23413
    {

23414
      SimStruct *rts = ssGetSFunction(S, 13);

23415
      if (rts != (NULL) ) {

23416
        rt_FREE(ssGetSampleTimePtr(rts));

23417
        rt_FREE(ssGetOffsetTimePtr(rts));

23418
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23419


23420
        /* Destroy model methods 3 */

23421
        {

23422
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23423
          rt_FREE(methods3);

23424
        }

23425


23426
        /* Destroy model methods 2 */

23427
        {

23428
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23429
          rt_FREE(methods2);

23430
        }

23431


23432
        /* Destroy state auxilliary information */

23433
        {

23434
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23435
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23436
          rt_FREE(periodicStatesInfo);

23437
          rt_FREE(statesInfo2);

23438
        }

23439


23440
        {

23441
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23442
          rt_FREE(ptr);

23443
        }

23444


23445
        {

23446
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23447
          rt_FREE(ptr);

23448
        }

23449


23450
        {

23451
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23452
          rt_FREE(ptr);

23453
        }

23454


23455
        {

23456
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

23457
          rt_FREE(ptr);

23458
        }

23459


23460
        {

23461
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

23462
          rt_FREE(ptr);

23463
        }

23464


23465
        {

23466
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

23467
          rt_FREE(ptr);

23468
        }

23469


23470
        {

23471
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

23472
          rt_FREE(ptr);

23473
        }

23474


23475
        rt_FREE(ssGetPortInfoForInputs(rts));

23476
        rt_FREE(ssGetPortInfoForOutputs(rts));

23477
        rt_FREE(ssGetSFcnDWork(rts));

23478
      }

23479
    }

23480


23481
    /* RTW Generated Level2 S-Function Block: '<S176>/Generated S-Function' (LagFilter_sf) */

23482
    {

23483
      SimStruct *rts = ssGetSFunction(S, 14);

23484
      if (rts != (NULL) ) {

23485
        rt_FREE(ssGetSampleTimePtr(rts));

23486
        rt_FREE(ssGetOffsetTimePtr(rts));

23487
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23488


23489
        /* Destroy model methods 3 */

23490
        {

23491
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23492
          rt_FREE(methods3);

23493
        }

23494


23495
        /* Destroy model methods 2 */

23496
        {

23497
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23498
          rt_FREE(methods2);

23499
        }

23500


23501
        /* Destroy state auxilliary information */

23502
        {

23503
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23504
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23505
          rt_FREE(periodicStatesInfo);

23506
          rt_FREE(statesInfo2);

23507
        }

23508


23509
        {

23510
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23511
          rt_FREE(ptr);

23512
        }

23513


23514
        {

23515
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23516
          rt_FREE(ptr);

23517
        }

23518


23519
        {

23520
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23521
          rt_FREE(ptr);

23522
        }

23523


23524
        {

23525
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

23526
          rt_FREE(ptr);

23527
        }

23528


23529
        {

23530
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

23531
          rt_FREE(ptr);

23532
        }

23533


23534
        {

23535
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

23536
          rt_FREE(ptr);

23537
        }

23538


23539
        rt_FREE(ssGetPortInfoForInputs(rts));

23540
        rt_FREE(ssGetPortInfoForOutputs(rts));

23541
        rt_FREE(ssGetSFcnDWork(rts));

23542
      }

23543
    }

23544


23545
    /* RTW Generated Level2 S-Function Block: '<S92>/Generated S-Function1' (SaturationLimiter_sf) */

23546
    {

23547
      SimStruct *rts = ssGetSFunction(S, 15);

23548
      if (rts != (NULL) ) {

23549
        rt_FREE(ssGetSampleTimePtr(rts));

23550
        rt_FREE(ssGetOffsetTimePtr(rts));

23551
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23552


23553
        /* Destroy model methods 3 */

23554
        {

23555
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23556
          rt_FREE(methods3);

23557
        }

23558


23559
        /* Destroy model methods 2 */

23560
        {

23561
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23562
          rt_FREE(methods2);

23563
        }

23564


23565
        /* Destroy state auxilliary information */

23566
        {

23567
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23568
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23569
          rt_FREE(periodicStatesInfo);

23570
          rt_FREE(statesInfo2);

23571
        }

23572


23573
        {

23574
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23575
          rt_FREE(ptr);

23576
        }

23577


23578
        {

23579
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23580
          rt_FREE(ptr);

23581
        }

23582


23583
        {

23584
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23585
          rt_FREE(ptr);

23586
        }

23587


23588
        rt_FREE(ssGetPortInfoForInputs(rts));

23589
        rt_FREE(ssGetPortInfoForOutputs(rts));

23590
      }

23591
    }

23592


23593
    /* RTW Generated Level2 S-Function Block: '<S96>/Generated S-Function' (Gain_sf) */

23594
    {

23595
      SimStruct *rts = ssGetSFunction(S, 16);

23596
      if (rts != (NULL) ) {

23597
        rt_FREE(ssGetSampleTimePtr(rts));

23598
        rt_FREE(ssGetOffsetTimePtr(rts));

23599
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23600


23601
        /* Destroy model methods 3 */

23602
        {

23603
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23604
          rt_FREE(methods3);

23605
        }

23606


23607
        /* Destroy model methods 2 */

23608
        {

23609
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23610
          rt_FREE(methods2);

23611
        }

23612


23613
        /* Destroy state auxilliary information */

23614
        {

23615
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23616
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23617
          rt_FREE(periodicStatesInfo);

23618
          rt_FREE(statesInfo2);

23619
        }

23620


23621
        {

23622
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23623
          rt_FREE(ptr);

23624
        }

23625


23626
        {

23627
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23628
          rt_FREE(ptr);

23629
        }

23630


23631
        rt_FREE(ssGetPortInfoForInputs(rts));

23632
        rt_FREE(ssGetPortInfoForOutputs(rts));

23633
      }

23634
    }

23635


23636
    /* RTW Generated Level2 S-Function Block: '<S93>/Generated S-Function1' (SaturationLimiter_sf) */

23637
    {

23638
      SimStruct *rts = ssGetSFunction(S, 17);

23639
      if (rts != (NULL) ) {

23640
        rt_FREE(ssGetSampleTimePtr(rts));

23641
        rt_FREE(ssGetOffsetTimePtr(rts));

23642
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23643


23644
        /* Destroy model methods 3 */

23645
        {

23646
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23647
          rt_FREE(methods3);

23648
        }

23649


23650
        /* Destroy model methods 2 */

23651
        {

23652
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23653
          rt_FREE(methods2);

23654
        }

23655


23656
        /* Destroy state auxilliary information */

23657
        {

23658
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23659
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23660
          rt_FREE(periodicStatesInfo);

23661
          rt_FREE(statesInfo2);

23662
        }

23663


23664
        {

23665
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23666
          rt_FREE(ptr);

23667
        }

23668


23669
        {

23670
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23671
          rt_FREE(ptr);

23672
        }

23673


23674
        {

23675
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23676
          rt_FREE(ptr);

23677
        }

23678


23679
        rt_FREE(ssGetPortInfoForInputs(rts));

23680
        rt_FREE(ssGetPortInfoForOutputs(rts));

23681
      }

23682
    }

23683


23684
    /* RTW Generated Level2 S-Function Block: '<S97>/Generated S-Function' (Gain_sf) */

23685
    {

23686
      SimStruct *rts = ssGetSFunction(S, 18);

23687
      if (rts != (NULL) ) {

23688
        rt_FREE(ssGetSampleTimePtr(rts));

23689
        rt_FREE(ssGetOffsetTimePtr(rts));

23690
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23691


23692
        /* Destroy model methods 3 */

23693
        {

23694
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23695
          rt_FREE(methods3);

23696
        }

23697


23698
        /* Destroy model methods 2 */

23699
        {

23700
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23701
          rt_FREE(methods2);

23702
        }

23703


23704
        /* Destroy state auxilliary information */

23705
        {

23706
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23707
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23708
          rt_FREE(periodicStatesInfo);

23709
          rt_FREE(statesInfo2);

23710
        }

23711


23712
        {

23713
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23714
          rt_FREE(ptr);

23715
        }

23716


23717
        {

23718
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23719
          rt_FREE(ptr);

23720
        }

23721


23722
        rt_FREE(ssGetPortInfoForInputs(rts));

23723
        rt_FREE(ssGetPortInfoForOutputs(rts));

23724
      }

23725
    }

23726


23727
    /* RTW Generated Level2 S-Function Block: '<S89>/Generated S-Function3' (IntegratorLimited_sf) */

23728
    {

23729
      SimStruct *rts = ssGetSFunction(S, 19);

23730
      if (rts != (NULL) ) {

23731
        rt_FREE(ssGetSampleTimePtr(rts));

23732
        rt_FREE(ssGetOffsetTimePtr(rts));

23733
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23734


23735
        /* Destroy model methods 3 */

23736
        {

23737
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23738
          rt_FREE(methods3);

23739
        }

23740


23741
        /* Destroy model methods 2 */

23742
        {

23743
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23744
          rt_FREE(methods2);

23745
        }

23746


23747
        /* Destroy state auxilliary information */

23748
        {

23749
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23750
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23751
          rt_FREE(periodicStatesInfo);

23752
          rt_FREE(statesInfo2);

23753
        }

23754


23755
        {

23756
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23757
          rt_FREE(ptr);

23758
        }

23759


23760
        {

23761
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23762
          rt_FREE(ptr);

23763
        }

23764


23765
        {

23766
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23767
          rt_FREE(ptr);

23768
        }

23769


23770
        {

23771
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

23772
          rt_FREE(ptr);

23773
        }

23774


23775
        {

23776
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

23777
          rt_FREE(ptr);

23778
        }

23779


23780
        {

23781
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

23782
          rt_FREE(ptr);

23783
        }

23784


23785
        {

23786
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

23787
          rt_FREE(ptr);

23788
        }

23789


23790
        rt_FREE(ssGetPortInfoForInputs(rts));

23791
        rt_FREE(ssGetPortInfoForOutputs(rts));

23792
        rt_FREE(ssGetSFcnDWork(rts));

23793
      }

23794
    }

23795


23796
    /* RTW Generated Level2 S-Function Block: '<S91>/Generated S-Function1' (SaturationLimiter_sf) */

23797
    {

23798
      SimStruct *rts = ssGetSFunction(S, 20);

23799
      if (rts != (NULL) ) {

23800
        rt_FREE(ssGetSampleTimePtr(rts));

23801
        rt_FREE(ssGetOffsetTimePtr(rts));

23802
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23803


23804
        /* Destroy model methods 3 */

23805
        {

23806
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23807
          rt_FREE(methods3);

23808
        }

23809


23810
        /* Destroy model methods 2 */

23811
        {

23812
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23813
          rt_FREE(methods2);

23814
        }

23815


23816
        /* Destroy state auxilliary information */

23817
        {

23818
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23819
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23820
          rt_FREE(periodicStatesInfo);

23821
          rt_FREE(statesInfo2);

23822
        }

23823


23824
        {

23825
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23826
          rt_FREE(ptr);

23827
        }

23828


23829
        {

23830
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23831
          rt_FREE(ptr);

23832
        }

23833


23834
        {

23835
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23836
          rt_FREE(ptr);

23837
        }

23838


23839
        rt_FREE(ssGetPortInfoForInputs(rts));

23840
        rt_FREE(ssGetPortInfoForOutputs(rts));

23841
      }

23842
    }

23843


23844
    /* RTW Generated Level2 S-Function Block: '<S95>/Generated S-Function' (Gain_sf) */

23845
    {

23846
      SimStruct *rts = ssGetSFunction(S, 21);

23847
      if (rts != (NULL) ) {

23848
        rt_FREE(ssGetSampleTimePtr(rts));

23849
        rt_FREE(ssGetOffsetTimePtr(rts));

23850
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23851


23852
        /* Destroy model methods 3 */

23853
        {

23854
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23855
          rt_FREE(methods3);

23856
        }

23857


23858
        /* Destroy model methods 2 */

23859
        {

23860
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23861
          rt_FREE(methods2);

23862
        }

23863


23864
        /* Destroy state auxilliary information */

23865
        {

23866
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23867
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23868
          rt_FREE(periodicStatesInfo);

23869
          rt_FREE(statesInfo2);

23870
        }

23871


23872
        {

23873
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23874
          rt_FREE(ptr);

23875
        }

23876


23877
        {

23878
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23879
          rt_FREE(ptr);

23880
        }

23881


23882
        rt_FREE(ssGetPortInfoForInputs(rts));

23883
        rt_FREE(ssGetPortInfoForOutputs(rts));

23884
      }

23885
    }

23886


23887
    /* RTW Generated Level2 S-Function Block: '<S90>/Generated S-Function2' (LeadLagFilter_sf) */

23888
    {

23889
      SimStruct *rts = ssGetSFunction(S, 22);

23890
      if (rts != (NULL) ) {

23891
        rt_FREE(ssGetSampleTimePtr(rts));

23892
        rt_FREE(ssGetOffsetTimePtr(rts));

23893
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23894


23895
        /* Destroy model methods 3 */

23896
        {

23897
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23898
          rt_FREE(methods3);

23899
        }

23900


23901
        /* Destroy model methods 2 */

23902
        {

23903
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23904
          rt_FREE(methods2);

23905
        }

23906


23907
        /* Destroy state auxilliary information */

23908
        {

23909
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23910
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23911
          rt_FREE(periodicStatesInfo);

23912
          rt_FREE(statesInfo2);

23913
        }

23914


23915
        {

23916
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23917
          rt_FREE(ptr);

23918
        }

23919


23920
        {

23921
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23922
          rt_FREE(ptr);

23923
        }

23924


23925
        {

23926
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23927
          rt_FREE(ptr);

23928
        }

23929


23930
        {

23931
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

23932
          rt_FREE(ptr);

23933
        }

23934


23935
        {

23936
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

23937
          rt_FREE(ptr);

23938
        }

23939


23940
        {

23941
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

23942
          rt_FREE(ptr);

23943
        }

23944


23945
        {

23946
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

23947
          rt_FREE(ptr);

23948
        }

23949


23950
        rt_FREE(ssGetPortInfoForInputs(rts));

23951
        rt_FREE(ssGetPortInfoForOutputs(rts));

23952
        rt_FREE(ssGetSFcnDWork(rts));

23953
      }

23954
    }

23955


23956
    /* RTW Generated Level2 S-Function Block: '<S94>/Generated S-Function1' (SaturationLimiter_sf) */

23957
    {

23958
      SimStruct *rts = ssGetSFunction(S, 23);

23959
      if (rts != (NULL) ) {

23960
        rt_FREE(ssGetSampleTimePtr(rts));

23961
        rt_FREE(ssGetOffsetTimePtr(rts));

23962
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

23963


23964
        /* Destroy model methods 3 */

23965
        {

23966
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

23967
          rt_FREE(methods3);

23968
        }

23969


23970
        /* Destroy model methods 2 */

23971
        {

23972
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

23973
          rt_FREE(methods2);

23974
        }

23975


23976
        /* Destroy state auxilliary information */

23977
        {

23978
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

23979
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

23980
          rt_FREE(periodicStatesInfo);

23981
          rt_FREE(statesInfo2);

23982
        }

23983


23984
        {

23985
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

23986
          rt_FREE(ptr);

23987
        }

23988


23989
        {

23990
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

23991
          rt_FREE(ptr);

23992
        }

23993


23994
        {

23995
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

23996
          rt_FREE(ptr);

23997
        }

23998


23999
        rt_FREE(ssGetPortInfoForInputs(rts));

24000
        rt_FREE(ssGetPortInfoForOutputs(rts));

24001
      }

24002
    }

24003


24004
    /* RTW Generated Level2 S-Function Block: '<S87>/Generated S-Function4' (LagFilter_sf) */

24005
    {

24006
      SimStruct *rts = ssGetSFunction(S, 24);

24007
      if (rts != (NULL) ) {

24008
        rt_FREE(ssGetSampleTimePtr(rts));

24009
        rt_FREE(ssGetOffsetTimePtr(rts));

24010
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24011


24012
        /* Destroy model methods 3 */

24013
        {

24014
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24015
          rt_FREE(methods3);

24016
        }

24017


24018
        /* Destroy model methods 2 */

24019
        {

24020
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24021
          rt_FREE(methods2);

24022
        }

24023


24024
        /* Destroy state auxilliary information */

24025
        {

24026
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24027
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24028
          rt_FREE(periodicStatesInfo);

24029
          rt_FREE(statesInfo2);

24030
        }

24031


24032
        {

24033
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24034
          rt_FREE(ptr);

24035
        }

24036


24037
        {

24038
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24039
          rt_FREE(ptr);

24040
        }

24041


24042
        {

24043
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24044
          rt_FREE(ptr);

24045
        }

24046


24047
        {

24048
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

24049
          rt_FREE(ptr);

24050
        }

24051


24052
        {

24053
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

24054
          rt_FREE(ptr);

24055
        }

24056


24057
        {

24058
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

24059
          rt_FREE(ptr);

24060
        }

24061


24062
        rt_FREE(ssGetPortInfoForInputs(rts));

24063
        rt_FREE(ssGetPortInfoForOutputs(rts));

24064
        rt_FREE(ssGetSFcnDWork(rts));

24065
      }

24066
    }

24067


24068
    /* RTW Generated Level2 S-Function Block: '<S114>/Generated S-Function' (SaturationLimiter_sf) */

24069
    {

24070
      SimStruct *rts = ssGetSFunction(S, 25);

24071
      if (rts != (NULL) ) {

24072
        rt_FREE(ssGetSampleTimePtr(rts));

24073
        rt_FREE(ssGetOffsetTimePtr(rts));

24074
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24075


24076
        /* Destroy model methods 3 */

24077
        {

24078
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24079
          rt_FREE(methods3);

24080
        }

24081


24082
        /* Destroy model methods 2 */

24083
        {

24084
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24085
          rt_FREE(methods2);

24086
        }

24087


24088
        /* Destroy state auxilliary information */

24089
        {

24090
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24091
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24092
          rt_FREE(periodicStatesInfo);

24093
          rt_FREE(statesInfo2);

24094
        }

24095


24096
        {

24097
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24098
          rt_FREE(ptr);

24099
        }

24100


24101
        {

24102
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24103
          rt_FREE(ptr);

24104
        }

24105


24106
        {

24107
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24108
          rt_FREE(ptr);

24109
        }

24110


24111
        rt_FREE(ssGetPortInfoForInputs(rts));

24112
        rt_FREE(ssGetPortInfoForOutputs(rts));

24113
      }

24114
    }

24115


24116
    /* RTW Generated Level2 S-Function Block: '<S113>/Generated S-Function' (LagFilter_sf) */

24117
    {

24118
      SimStruct *rts = ssGetSFunction(S, 26);

24119
      if (rts != (NULL) ) {

24120
        rt_FREE(ssGetSampleTimePtr(rts));

24121
        rt_FREE(ssGetOffsetTimePtr(rts));

24122
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24123


24124
        /* Destroy model methods 3 */

24125
        {

24126
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24127
          rt_FREE(methods3);

24128
        }

24129


24130
        /* Destroy model methods 2 */

24131
        {

24132
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24133
          rt_FREE(methods2);

24134
        }

24135


24136
        /* Destroy state auxilliary information */

24137
        {

24138
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24139
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24140
          rt_FREE(periodicStatesInfo);

24141
          rt_FREE(statesInfo2);

24142
        }

24143


24144
        {

24145
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24146
          rt_FREE(ptr);

24147
        }

24148


24149
        {

24150
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24151
          rt_FREE(ptr);

24152
        }

24153


24154
        {

24155
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24156
          rt_FREE(ptr);

24157
        }

24158


24159
        {

24160
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

24161
          rt_FREE(ptr);

24162
        }

24163


24164
        {

24165
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

24166
          rt_FREE(ptr);

24167
        }

24168


24169
        {

24170
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

24171
          rt_FREE(ptr);

24172
        }

24173


24174
        rt_FREE(ssGetPortInfoForInputs(rts));

24175
        rt_FREE(ssGetPortInfoForOutputs(rts));

24176
        rt_FREE(ssGetSFcnDWork(rts));

24177
      }

24178
    }

24179


24180
    /* RTW Generated Level2 S-Function Block: '<S86>/Generated S-Function6' (Res180_sf) */

24181
    {

24182
      SimStruct *rts = ssGetSFunction(S, 27);

24183
      if (rts != (NULL) ) {

24184
        rt_FREE(ssGetSampleTimePtr(rts));

24185
        rt_FREE(ssGetOffsetTimePtr(rts));

24186
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24187


24188
        /* Destroy model methods 3 */

24189
        {

24190
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24191
          rt_FREE(methods3);

24192
        }

24193


24194
        /* Destroy model methods 2 */

24195
        {

24196
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24197
          rt_FREE(methods2);

24198
        }

24199


24200
        /* Destroy state auxilliary information */

24201
        {

24202
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24203
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24204
          rt_FREE(periodicStatesInfo);

24205
          rt_FREE(statesInfo2);

24206
        }

24207


24208
        {

24209
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24210
          rt_FREE(ptr);

24211
        }

24212


24213
        rt_FREE(ssGetPortInfoForInputs(rts));

24214
        rt_FREE(ssGetPortInfoForOutputs(rts));

24215
      }

24216
    }

24217


24218
    /* RTW Generated Level2 S-Function Block: '<S119>/Generated S-Function' (SaturationLimiter_sf) */

24219
    {

24220
      SimStruct *rts = ssGetSFunction(S, 28);

24221
      if (rts != (NULL) ) {

24222
        rt_FREE(ssGetSampleTimePtr(rts));

24223
        rt_FREE(ssGetOffsetTimePtr(rts));

24224
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24225


24226
        /* Destroy model methods 3 */

24227
        {

24228
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24229
          rt_FREE(methods3);

24230
        }

24231


24232
        /* Destroy model methods 2 */

24233
        {

24234
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24235
          rt_FREE(methods2);

24236
        }

24237


24238
        /* Destroy state auxilliary information */

24239
        {

24240
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24241
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24242
          rt_FREE(periodicStatesInfo);

24243
          rt_FREE(statesInfo2);

24244
        }

24245


24246
        {

24247
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24248
          rt_FREE(ptr);

24249
        }

24250


24251
        {

24252
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24253
          rt_FREE(ptr);

24254
        }

24255


24256
        {

24257
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24258
          rt_FREE(ptr);

24259
        }

24260


24261
        rt_FREE(ssGetPortInfoForInputs(rts));

24262
        rt_FREE(ssGetPortInfoForOutputs(rts));

24263
      }

24264
    }

24265


24266
    /* RTW Generated Level2 S-Function Block: '<S121>/Generated S-Function' (Gain_sf) */

24267
    {

24268
      SimStruct *rts = ssGetSFunction(S, 29);

24269
      if (rts != (NULL) ) {

24270
        rt_FREE(ssGetSampleTimePtr(rts));

24271
        rt_FREE(ssGetOffsetTimePtr(rts));

24272
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24273


24274
        /* Destroy model methods 3 */

24275
        {

24276
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24277
          rt_FREE(methods3);

24278
        }

24279


24280
        /* Destroy model methods 2 */

24281
        {

24282
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24283
          rt_FREE(methods2);

24284
        }

24285


24286
        /* Destroy state auxilliary information */

24287
        {

24288
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24289
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24290
          rt_FREE(periodicStatesInfo);

24291
          rt_FREE(statesInfo2);

24292
        }

24293


24294
        {

24295
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24296
          rt_FREE(ptr);

24297
        }

24298


24299
        {

24300
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24301
          rt_FREE(ptr);

24302
        }

24303


24304
        rt_FREE(ssGetPortInfoForInputs(rts));

24305
        rt_FREE(ssGetPortInfoForOutputs(rts));

24306
      }

24307
    }

24308


24309
    /* RTW Generated Level2 S-Function Block: '<S118>/Generated S-Function2' (LeadLagFilter_sf) */

24310
    {

24311
      SimStruct *rts = ssGetSFunction(S, 30);

24312
      if (rts != (NULL) ) {

24313
        rt_FREE(ssGetSampleTimePtr(rts));

24314
        rt_FREE(ssGetOffsetTimePtr(rts));

24315
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24316


24317
        /* Destroy model methods 3 */

24318
        {

24319
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24320
          rt_FREE(methods3);

24321
        }

24322


24323
        /* Destroy model methods 2 */

24324
        {

24325
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24326
          rt_FREE(methods2);

24327
        }

24328


24329
        /* Destroy state auxilliary information */

24330
        {

24331
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24332
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24333
          rt_FREE(periodicStatesInfo);

24334
          rt_FREE(statesInfo2);

24335
        }

24336


24337
        {

24338
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24339
          rt_FREE(ptr);

24340
        }

24341


24342
        {

24343
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24344
          rt_FREE(ptr);

24345
        }

24346


24347
        {

24348
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24349
          rt_FREE(ptr);

24350
        }

24351


24352
        {

24353
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

24354
          rt_FREE(ptr);

24355
        }

24356


24357
        {

24358
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

24359
          rt_FREE(ptr);

24360
        }

24361


24362
        {

24363
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

24364
          rt_FREE(ptr);

24365
        }

24366


24367
        {

24368
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

24369
          rt_FREE(ptr);

24370
        }

24371


24372
        rt_FREE(ssGetPortInfoForInputs(rts));

24373
        rt_FREE(ssGetPortInfoForOutputs(rts));

24374
        rt_FREE(ssGetSFcnDWork(rts));

24375
      }

24376
    }

24377


24378
    /* RTW Generated Level2 S-Function Block: '<S120>/Generated S-Function1' (SaturationLimiter_sf) */

24379
    {

24380
      SimStruct *rts = ssGetSFunction(S, 31);

24381
      if (rts != (NULL) ) {

24382
        rt_FREE(ssGetSampleTimePtr(rts));

24383
        rt_FREE(ssGetOffsetTimePtr(rts));

24384
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24385


24386
        /* Destroy model methods 3 */

24387
        {

24388
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24389
          rt_FREE(methods3);

24390
        }

24391


24392
        /* Destroy model methods 2 */

24393
        {

24394
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24395
          rt_FREE(methods2);

24396
        }

24397


24398
        /* Destroy state auxilliary information */

24399
        {

24400
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24401
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24402
          rt_FREE(periodicStatesInfo);

24403
          rt_FREE(statesInfo2);

24404
        }

24405


24406
        {

24407
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24408
          rt_FREE(ptr);

24409
        }

24410


24411
        {

24412
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24413
          rt_FREE(ptr);

24414
        }

24415


24416
        {

24417
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24418
          rt_FREE(ptr);

24419
        }

24420


24421
        rt_FREE(ssGetPortInfoForInputs(rts));

24422
        rt_FREE(ssGetPortInfoForOutputs(rts));

24423
      }

24424
    }

24425


24426
    /* RTW Generated Level2 S-Function Block: '<S117>/Generated S-Function4' (LagFilter_sf) */

24427
    {

24428
      SimStruct *rts = ssGetSFunction(S, 32);

24429
      if (rts != (NULL) ) {

24430
        rt_FREE(ssGetSampleTimePtr(rts));

24431
        rt_FREE(ssGetOffsetTimePtr(rts));

24432
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24433


24434
        /* Destroy model methods 3 */

24435
        {

24436
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24437
          rt_FREE(methods3);

24438
        }

24439


24440
        /* Destroy model methods 2 */

24441
        {

24442
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24443
          rt_FREE(methods2);

24444
        }

24445


24446
        /* Destroy state auxilliary information */

24447
        {

24448
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24449
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24450
          rt_FREE(periodicStatesInfo);

24451
          rt_FREE(statesInfo2);

24452
        }

24453


24454
        {

24455
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24456
          rt_FREE(ptr);

24457
        }

24458


24459
        {

24460
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24461
          rt_FREE(ptr);

24462
        }

24463


24464
        {

24465
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24466
          rt_FREE(ptr);

24467
        }

24468


24469
        {

24470
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

24471
          rt_FREE(ptr);

24472
        }

24473


24474
        {

24475
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

24476
          rt_FREE(ptr);

24477
        }

24478


24479
        {

24480
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

24481
          rt_FREE(ptr);

24482
        }

24483


24484
        rt_FREE(ssGetPortInfoForInputs(rts));

24485
        rt_FREE(ssGetPortInfoForOutputs(rts));

24486
        rt_FREE(ssGetSFcnDWork(rts));

24487
      }

24488
    }

24489


24490
    /* RTW Generated Level2 S-Function Block: '<S122>/Generated S-Function' (Gain_sf) */

24491
    {

24492
      SimStruct *rts = ssGetSFunction(S, 33);

24493
      if (rts != (NULL) ) {

24494
        rt_FREE(ssGetSampleTimePtr(rts));

24495
        rt_FREE(ssGetOffsetTimePtr(rts));

24496
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24497


24498
        /* Destroy model methods 3 */

24499
        {

24500
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24501
          rt_FREE(methods3);

24502
        }

24503


24504
        /* Destroy model methods 2 */

24505
        {

24506
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24507
          rt_FREE(methods2);

24508
        }

24509


24510
        /* Destroy state auxilliary information */

24511
        {

24512
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24513
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24514
          rt_FREE(periodicStatesInfo);

24515
          rt_FREE(statesInfo2);

24516
        }

24517


24518
        {

24519
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24520
          rt_FREE(ptr);

24521
        }

24522


24523
        {

24524
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24525
          rt_FREE(ptr);

24526
        }

24527


24528
        rt_FREE(ssGetPortInfoForInputs(rts));

24529
        rt_FREE(ssGetPortInfoForOutputs(rts));

24530
      }

24531
    }

24532


24533
    /* RTW Generated Level2 S-Function Block: '<S116>/Generated S-Function4' (LagFilter_sf) */

24534
    {

24535
      SimStruct *rts = ssGetSFunction(S, 34);

24536
      if (rts != (NULL) ) {

24537
        rt_FREE(ssGetSampleTimePtr(rts));

24538
        rt_FREE(ssGetOffsetTimePtr(rts));

24539
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24540


24541
        /* Destroy model methods 3 */

24542
        {

24543
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24544
          rt_FREE(methods3);

24545
        }

24546


24547
        /* Destroy model methods 2 */

24548
        {

24549
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24550
          rt_FREE(methods2);

24551
        }

24552


24553
        /* Destroy state auxilliary information */

24554
        {

24555
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24556
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24557
          rt_FREE(periodicStatesInfo);

24558
          rt_FREE(statesInfo2);

24559
        }

24560


24561
        {

24562
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24563
          rt_FREE(ptr);

24564
        }

24565


24566
        {

24567
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24568
          rt_FREE(ptr);

24569
        }

24570


24571
        {

24572
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24573
          rt_FREE(ptr);

24574
        }

24575


24576
        {

24577
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

24578
          rt_FREE(ptr);

24579
        }

24580


24581
        {

24582
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

24583
          rt_FREE(ptr);

24584
        }

24585


24586
        {

24587
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

24588
          rt_FREE(ptr);

24589
        }

24590


24591
        rt_FREE(ssGetPortInfoForInputs(rts));

24592
        rt_FREE(ssGetPortInfoForOutputs(rts));

24593
        rt_FREE(ssGetSFcnDWork(rts));

24594
      }

24595
    }

24596


24597
    /* RTW Generated Level2 S-Function Block: '<S102>/Generated S-Function1' (SaturationLimiter_sf) */

24598
    {

24599
      SimStruct *rts = ssGetSFunction(S, 35);

24600
      if (rts != (NULL) ) {

24601
        rt_FREE(ssGetSampleTimePtr(rts));

24602
        rt_FREE(ssGetOffsetTimePtr(rts));

24603
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24604


24605
        /* Destroy model methods 3 */

24606
        {

24607
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24608
          rt_FREE(methods3);

24609
        }

24610


24611
        /* Destroy model methods 2 */

24612
        {

24613
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24614
          rt_FREE(methods2);

24615
        }

24616


24617
        /* Destroy state auxilliary information */

24618
        {

24619
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24620
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24621
          rt_FREE(periodicStatesInfo);

24622
          rt_FREE(statesInfo2);

24623
        }

24624


24625
        {

24626
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24627
          rt_FREE(ptr);

24628
        }

24629


24630
        {

24631
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24632
          rt_FREE(ptr);

24633
        }

24634


24635
        {

24636
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24637
          rt_FREE(ptr);

24638
        }

24639


24640
        rt_FREE(ssGetPortInfoForInputs(rts));

24641
        rt_FREE(ssGetPortInfoForOutputs(rts));

24642
      }

24643
    }

24644


24645
    /* RTW Generated Level2 S-Function Block: '<S104>/Generated S-Function' (Gain_sf) */

24646
    {

24647
      SimStruct *rts = ssGetSFunction(S, 36);

24648
      if (rts != (NULL) ) {

24649
        rt_FREE(ssGetSampleTimePtr(rts));

24650
        rt_FREE(ssGetOffsetTimePtr(rts));

24651
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24652


24653
        /* Destroy model methods 3 */

24654
        {

24655
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24656
          rt_FREE(methods3);

24657
        }

24658


24659
        /* Destroy model methods 2 */

24660
        {

24661
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24662
          rt_FREE(methods2);

24663
        }

24664


24665
        /* Destroy state auxilliary information */

24666
        {

24667
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24668
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24669
          rt_FREE(periodicStatesInfo);

24670
          rt_FREE(statesInfo2);

24671
        }

24672


24673
        {

24674
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24675
          rt_FREE(ptr);

24676
        }

24677


24678
        {

24679
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24680
          rt_FREE(ptr);

24681
        }

24682


24683
        rt_FREE(ssGetPortInfoForInputs(rts));

24684
        rt_FREE(ssGetPortInfoForOutputs(rts));

24685
      }

24686
    }

24687


24688
    /* RTW Generated Level2 S-Function Block: '<S105>/Generated S-Function' (Gain_sf) */

24689
    {

24690
      SimStruct *rts = ssGetSFunction(S, 37);

24691
      if (rts != (NULL) ) {

24692
        rt_FREE(ssGetSampleTimePtr(rts));

24693
        rt_FREE(ssGetOffsetTimePtr(rts));

24694
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24695


24696
        /* Destroy model methods 3 */

24697
        {

24698
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24699
          rt_FREE(methods3);

24700
        }

24701


24702
        /* Destroy model methods 2 */

24703
        {

24704
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24705
          rt_FREE(methods2);

24706
        }

24707


24708
        /* Destroy state auxilliary information */

24709
        {

24710
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24711
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24712
          rt_FREE(periodicStatesInfo);

24713
          rt_FREE(statesInfo2);

24714
        }

24715


24716
        {

24717
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24718
          rt_FREE(ptr);

24719
        }

24720


24721
        {

24722
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24723
          rt_FREE(ptr);

24724
        }

24725


24726
        rt_FREE(ssGetPortInfoForInputs(rts));

24727
        rt_FREE(ssGetPortInfoForOutputs(rts));

24728
      }

24729
    }

24730


24731
    /* RTW Generated Level2 S-Function Block: '<S100>/Generated S-Function3' (IntegratorLimited_sf) */

24732
    {

24733
      SimStruct *rts = ssGetSFunction(S, 38);

24734
      if (rts != (NULL) ) {

24735
        rt_FREE(ssGetSampleTimePtr(rts));

24736
        rt_FREE(ssGetOffsetTimePtr(rts));

24737
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24738


24739
        /* Destroy model methods 3 */

24740
        {

24741
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24742
          rt_FREE(methods3);

24743
        }

24744


24745
        /* Destroy model methods 2 */

24746
        {

24747
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24748
          rt_FREE(methods2);

24749
        }

24750


24751
        /* Destroy state auxilliary information */

24752
        {

24753
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24754
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24755
          rt_FREE(periodicStatesInfo);

24756
          rt_FREE(statesInfo2);

24757
        }

24758


24759
        {

24760
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24761
          rt_FREE(ptr);

24762
        }

24763


24764
        {

24765
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24766
          rt_FREE(ptr);

24767
        }

24768


24769
        {

24770
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24771
          rt_FREE(ptr);

24772
        }

24773


24774
        {

24775
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

24776
          rt_FREE(ptr);

24777
        }

24778


24779
        {

24780
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

24781
          rt_FREE(ptr);

24782
        }

24783


24784
        {

24785
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

24786
          rt_FREE(ptr);

24787
        }

24788


24789
        {

24790
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

24791
          rt_FREE(ptr);

24792
        }

24793


24794
        rt_FREE(ssGetPortInfoForInputs(rts));

24795
        rt_FREE(ssGetPortInfoForOutputs(rts));

24796
        rt_FREE(ssGetSFcnDWork(rts));

24797
      }

24798
    }

24799


24800
    /* RTW Generated Level2 S-Function Block: '<S101>/Generated S-Function2' (LeadLagFilter_sf) */

24801
    {

24802
      SimStruct *rts = ssGetSFunction(S, 39);

24803
      if (rts != (NULL) ) {

24804
        rt_FREE(ssGetSampleTimePtr(rts));

24805
        rt_FREE(ssGetOffsetTimePtr(rts));

24806
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24807


24808
        /* Destroy model methods 3 */

24809
        {

24810
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24811
          rt_FREE(methods3);

24812
        }

24813


24814
        /* Destroy model methods 2 */

24815
        {

24816
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24817
          rt_FREE(methods2);

24818
        }

24819


24820
        /* Destroy state auxilliary information */

24821
        {

24822
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24823
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24824
          rt_FREE(periodicStatesInfo);

24825
          rt_FREE(statesInfo2);

24826
        }

24827


24828
        {

24829
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24830
          rt_FREE(ptr);

24831
        }

24832


24833
        {

24834
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24835
          rt_FREE(ptr);

24836
        }

24837


24838
        {

24839
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24840
          rt_FREE(ptr);

24841
        }

24842


24843
        {

24844
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

24845
          rt_FREE(ptr);

24846
        }

24847


24848
        {

24849
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

24850
          rt_FREE(ptr);

24851
        }

24852


24853
        {

24854
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

24855
          rt_FREE(ptr);

24856
        }

24857


24858
        {

24859
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

24860
          rt_FREE(ptr);

24861
        }

24862


24863
        rt_FREE(ssGetPortInfoForInputs(rts));

24864
        rt_FREE(ssGetPortInfoForOutputs(rts));

24865
        rt_FREE(ssGetSFcnDWork(rts));

24866
      }

24867
    }

24868


24869
    /* RTW Generated Level2 S-Function Block: '<S103>/Generated S-Function1' (SaturationLimiter_sf) */

24870
    {

24871
      SimStruct *rts = ssGetSFunction(S, 40);

24872
      if (rts != (NULL) ) {

24873
        rt_FREE(ssGetSampleTimePtr(rts));

24874
        rt_FREE(ssGetOffsetTimePtr(rts));

24875
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24876


24877
        /* Destroy model methods 3 */

24878
        {

24879
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24880
          rt_FREE(methods3);

24881
        }

24882


24883
        /* Destroy model methods 2 */

24884
        {

24885
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24886
          rt_FREE(methods2);

24887
        }

24888


24889
        /* Destroy state auxilliary information */

24890
        {

24891
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24892
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24893
          rt_FREE(periodicStatesInfo);

24894
          rt_FREE(statesInfo2);

24895
        }

24896


24897
        {

24898
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24899
          rt_FREE(ptr);

24900
        }

24901


24902
        {

24903
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24904
          rt_FREE(ptr);

24905
        }

24906


24907
        {

24908
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24909
          rt_FREE(ptr);

24910
        }

24911


24912
        rt_FREE(ssGetPortInfoForInputs(rts));

24913
        rt_FREE(ssGetPortInfoForOutputs(rts));

24914
      }

24915
    }

24916


24917
    /* RTW Generated Level2 S-Function Block: '<S98>/Generated S-Function4' (LagFilter_sf) */

24918
    {

24919
      SimStruct *rts = ssGetSFunction(S, 41);

24920
      if (rts != (NULL) ) {

24921
        rt_FREE(ssGetSampleTimePtr(rts));

24922
        rt_FREE(ssGetOffsetTimePtr(rts));

24923
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24924


24925
        /* Destroy model methods 3 */

24926
        {

24927
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24928
          rt_FREE(methods3);

24929
        }

24930


24931
        /* Destroy model methods 2 */

24932
        {

24933
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24934
          rt_FREE(methods2);

24935
        }

24936


24937
        /* Destroy state auxilliary information */

24938
        {

24939
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

24940
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

24941
          rt_FREE(periodicStatesInfo);

24942
          rt_FREE(statesInfo2);

24943
        }

24944


24945
        {

24946
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

24947
          rt_FREE(ptr);

24948
        }

24949


24950
        {

24951
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

24952
          rt_FREE(ptr);

24953
        }

24954


24955
        {

24956
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

24957
          rt_FREE(ptr);

24958
        }

24959


24960
        {

24961
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

24962
          rt_FREE(ptr);

24963
        }

24964


24965
        {

24966
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

24967
          rt_FREE(ptr);

24968
        }

24969


24970
        {

24971
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

24972
          rt_FREE(ptr);

24973
        }

24974


24975
        rt_FREE(ssGetPortInfoForInputs(rts));

24976
        rt_FREE(ssGetPortInfoForOutputs(rts));

24977
        rt_FREE(ssGetSFcnDWork(rts));

24978
      }

24979
    }

24980


24981
    /* RTW Generated Level2 S-Function Block: '<S109>/Generated S-Function1' (SaturationLimiter_sf) */

24982
    {

24983
      SimStruct *rts = ssGetSFunction(S, 42);

24984
      if (rts != (NULL) ) {

24985
        rt_FREE(ssGetSampleTimePtr(rts));

24986
        rt_FREE(ssGetOffsetTimePtr(rts));

24987
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

24988


24989
        /* Destroy model methods 3 */

24990
        {

24991
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

24992
          rt_FREE(methods3);

24993
        }

24994


24995
        /* Destroy model methods 2 */

24996
        {

24997
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

24998
          rt_FREE(methods2);

24999
        }

25000


25001
        /* Destroy state auxilliary information */

25002
        {

25003
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25004
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25005
          rt_FREE(periodicStatesInfo);

25006
          rt_FREE(statesInfo2);

25007
        }

25008


25009
        {

25010
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25011
          rt_FREE(ptr);

25012
        }

25013


25014
        {

25015
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25016
          rt_FREE(ptr);

25017
        }

25018


25019
        {

25020
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25021
          rt_FREE(ptr);

25022
        }

25023


25024
        rt_FREE(ssGetPortInfoForInputs(rts));

25025
        rt_FREE(ssGetPortInfoForOutputs(rts));

25026
      }

25027
    }

25028


25029
    /* RTW Generated Level2 S-Function Block: '<S111>/Generated S-Function' (Gain_sf) */

25030
    {

25031
      SimStruct *rts = ssGetSFunction(S, 43);

25032
      if (rts != (NULL) ) {

25033
        rt_FREE(ssGetSampleTimePtr(rts));

25034
        rt_FREE(ssGetOffsetTimePtr(rts));

25035
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25036


25037
        /* Destroy model methods 3 */

25038
        {

25039
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25040
          rt_FREE(methods3);

25041
        }

25042


25043
        /* Destroy model methods 2 */

25044
        {

25045
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25046
          rt_FREE(methods2);

25047
        }

25048


25049
        /* Destroy state auxilliary information */

25050
        {

25051
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25052
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25053
          rt_FREE(periodicStatesInfo);

25054
          rt_FREE(statesInfo2);

25055
        }

25056


25057
        {

25058
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25059
          rt_FREE(ptr);

25060
        }

25061


25062
        {

25063
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25064
          rt_FREE(ptr);

25065
        }

25066


25067
        rt_FREE(ssGetPortInfoForInputs(rts));

25068
        rt_FREE(ssGetPortInfoForOutputs(rts));

25069
      }

25070
    }

25071


25072
    /* RTW Generated Level2 S-Function Block: '<S112>/Generated S-Function' (Gain_sf) */

25073
    {

25074
      SimStruct *rts = ssGetSFunction(S, 44);

25075
      if (rts != (NULL) ) {

25076
        rt_FREE(ssGetSampleTimePtr(rts));

25077
        rt_FREE(ssGetOffsetTimePtr(rts));

25078
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25079


25080
        /* Destroy model methods 3 */

25081
        {

25082
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25083
          rt_FREE(methods3);

25084
        }

25085


25086
        /* Destroy model methods 2 */

25087
        {

25088
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25089
          rt_FREE(methods2);

25090
        }

25091


25092
        /* Destroy state auxilliary information */

25093
        {

25094
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25095
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25096
          rt_FREE(periodicStatesInfo);

25097
          rt_FREE(statesInfo2);

25098
        }

25099


25100
        {

25101
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25102
          rt_FREE(ptr);

25103
        }

25104


25105
        {

25106
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25107
          rt_FREE(ptr);

25108
        }

25109


25110
        rt_FREE(ssGetPortInfoForInputs(rts));

25111
        rt_FREE(ssGetPortInfoForOutputs(rts));

25112
      }

25113
    }

25114


25115
    /* RTW Generated Level2 S-Function Block: '<S107>/Generated S-Function3' (IntegratorLimited_sf) */

25116
    {

25117
      SimStruct *rts = ssGetSFunction(S, 45);

25118
      if (rts != (NULL) ) {

25119
        rt_FREE(ssGetSampleTimePtr(rts));

25120
        rt_FREE(ssGetOffsetTimePtr(rts));

25121
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25122


25123
        /* Destroy model methods 3 */

25124
        {

25125
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25126
          rt_FREE(methods3);

25127
        }

25128


25129
        /* Destroy model methods 2 */

25130
        {

25131
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25132
          rt_FREE(methods2);

25133
        }

25134


25135
        /* Destroy state auxilliary information */

25136
        {

25137
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25138
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25139
          rt_FREE(periodicStatesInfo);

25140
          rt_FREE(statesInfo2);

25141
        }

25142


25143
        {

25144
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25145
          rt_FREE(ptr);

25146
        }

25147


25148
        {

25149
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25150
          rt_FREE(ptr);

25151
        }

25152


25153
        {

25154
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25155
          rt_FREE(ptr);

25156
        }

25157


25158
        {

25159
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

25160
          rt_FREE(ptr);

25161
        }

25162


25163
        {

25164
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

25165
          rt_FREE(ptr);

25166
        }

25167


25168
        {

25169
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

25170
          rt_FREE(ptr);

25171
        }

25172


25173
        {

25174
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

25175
          rt_FREE(ptr);

25176
        }

25177


25178
        rt_FREE(ssGetPortInfoForInputs(rts));

25179
        rt_FREE(ssGetPortInfoForOutputs(rts));

25180
        rt_FREE(ssGetSFcnDWork(rts));

25181
      }

25182
    }

25183


25184
    /* RTW Generated Level2 S-Function Block: '<S108>/Generated S-Function2' (LeadLagFilter_sf) */

25185
    {

25186
      SimStruct *rts = ssGetSFunction(S, 46);

25187
      if (rts != (NULL) ) {

25188
        rt_FREE(ssGetSampleTimePtr(rts));

25189
        rt_FREE(ssGetOffsetTimePtr(rts));

25190
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25191


25192
        /* Destroy model methods 3 */

25193
        {

25194
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25195
          rt_FREE(methods3);

25196
        }

25197


25198
        /* Destroy model methods 2 */

25199
        {

25200
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25201
          rt_FREE(methods2);

25202
        }

25203


25204
        /* Destroy state auxilliary information */

25205
        {

25206
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25207
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25208
          rt_FREE(periodicStatesInfo);

25209
          rt_FREE(statesInfo2);

25210
        }

25211


25212
        {

25213
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25214
          rt_FREE(ptr);

25215
        }

25216


25217
        {

25218
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25219
          rt_FREE(ptr);

25220
        }

25221


25222
        {

25223
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25224
          rt_FREE(ptr);

25225
        }

25226


25227
        {

25228
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

25229
          rt_FREE(ptr);

25230
        }

25231


25232
        {

25233
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

25234
          rt_FREE(ptr);

25235
        }

25236


25237
        {

25238
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

25239
          rt_FREE(ptr);

25240
        }

25241


25242
        {

25243
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

25244
          rt_FREE(ptr);

25245
        }

25246


25247
        rt_FREE(ssGetPortInfoForInputs(rts));

25248
        rt_FREE(ssGetPortInfoForOutputs(rts));

25249
        rt_FREE(ssGetSFcnDWork(rts));

25250
      }

25251
    }

25252


25253
    /* RTW Generated Level2 S-Function Block: '<S110>/Generated S-Function1' (SaturationLimiter_sf) */

25254
    {

25255
      SimStruct *rts = ssGetSFunction(S, 47);

25256
      if (rts != (NULL) ) {

25257
        rt_FREE(ssGetSampleTimePtr(rts));

25258
        rt_FREE(ssGetOffsetTimePtr(rts));

25259
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25260


25261
        /* Destroy model methods 3 */

25262
        {

25263
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25264
          rt_FREE(methods3);

25265
        }

25266


25267
        /* Destroy model methods 2 */

25268
        {

25269
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25270
          rt_FREE(methods2);

25271
        }

25272


25273
        /* Destroy state auxilliary information */

25274
        {

25275
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25276
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25277
          rt_FREE(periodicStatesInfo);

25278
          rt_FREE(statesInfo2);

25279
        }

25280


25281
        {

25282
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25283
          rt_FREE(ptr);

25284
        }

25285


25286
        {

25287
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25288
          rt_FREE(ptr);

25289
        }

25290


25291
        {

25292
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25293
          rt_FREE(ptr);

25294
        }

25295


25296
        rt_FREE(ssGetPortInfoForInputs(rts));

25297
        rt_FREE(ssGetPortInfoForOutputs(rts));

25298
      }

25299
    }

25300


25301
    /* RTW Generated Level2 S-Function Block: '<S106>/Generated S-Function4' (LagFilter_sf) */

25302
    {

25303
      SimStruct *rts = ssGetSFunction(S, 48);

25304
      if (rts != (NULL) ) {

25305
        rt_FREE(ssGetSampleTimePtr(rts));

25306
        rt_FREE(ssGetOffsetTimePtr(rts));

25307
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25308


25309
        /* Destroy model methods 3 */

25310
        {

25311
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25312
          rt_FREE(methods3);

25313
        }

25314


25315
        /* Destroy model methods 2 */

25316
        {

25317
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25318
          rt_FREE(methods2);

25319
        }

25320


25321
        /* Destroy state auxilliary information */

25322
        {

25323
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25324
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25325
          rt_FREE(periodicStatesInfo);

25326
          rt_FREE(statesInfo2);

25327
        }

25328


25329
        {

25330
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25331
          rt_FREE(ptr);

25332
        }

25333


25334
        {

25335
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25336
          rt_FREE(ptr);

25337
        }

25338


25339
        {

25340
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25341
          rt_FREE(ptr);

25342
        }

25343


25344
        {

25345
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

25346
          rt_FREE(ptr);

25347
        }

25348


25349
        {

25350
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

25351
          rt_FREE(ptr);

25352
        }

25353


25354
        {

25355
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

25356
          rt_FREE(ptr);

25357
        }

25358


25359
        rt_FREE(ssGetPortInfoForInputs(rts));

25360
        rt_FREE(ssGetPortInfoForOutputs(rts));

25361
        rt_FREE(ssGetSFcnDWork(rts));

25362
      }

25363
    }

25364


25365
    /* RTW Generated Level2 S-Function Block: '<S81>/Generated S-Function5' (Fader_sf) */

25366
    {

25367
      SimStruct *rts = ssGetSFunction(S, 49);

25368
      if (rts != (NULL) ) {

25369
        rt_FREE(ssGetSampleTimePtr(rts));

25370
        rt_FREE(ssGetOffsetTimePtr(rts));

25371
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25372


25373
        /* Destroy model methods 3 */

25374
        {

25375
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25376
          rt_FREE(methods3);

25377
        }

25378


25379
        /* Destroy model methods 2 */

25380
        {

25381
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25382
          rt_FREE(methods2);

25383
        }

25384


25385
        /* Destroy state auxilliary information */

25386
        {

25387
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25388
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25389
          rt_FREE(periodicStatesInfo);

25390
          rt_FREE(statesInfo2);

25391
        }

25392


25393
        {

25394
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25395
          rt_FREE(ptr);

25396
        }

25397


25398
        {

25399
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25400
          rt_FREE(ptr);

25401
        }

25402


25403
        {

25404
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25405
          rt_FREE(ptr);

25406
        }

25407


25408
        {

25409
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

25410
          rt_FREE(ptr);

25411
        }

25412


25413
        {

25414
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

25415
          rt_FREE(ptr);

25416
        }

25417


25418
        {

25419
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

25420
          rt_FREE(ptr);

25421
        }

25422


25423
        rt_FREE(ssGetPortInfoForInputs(rts));

25424
        rt_FREE(ssGetPortInfoForOutputs(rts));

25425
        rt_FREE(ssGetSFcnDWork(rts));

25426
      }

25427
    }

25428


25429
    /* RTW Generated Level2 S-Function Block: '<S130>/Generated S-Function4' (LagFilter_sf) */

25430
    {

25431
      SimStruct *rts = ssGetSFunction(S, 50);

25432
      if (rts != (NULL) ) {

25433
        rt_FREE(ssGetSampleTimePtr(rts));

25434
        rt_FREE(ssGetOffsetTimePtr(rts));

25435
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25436


25437
        /* Destroy model methods 3 */

25438
        {

25439
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25440
          rt_FREE(methods3);

25441
        }

25442


25443
        /* Destroy model methods 2 */

25444
        {

25445
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25446
          rt_FREE(methods2);

25447
        }

25448


25449
        /* Destroy state auxilliary information */

25450
        {

25451
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25452
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25453
          rt_FREE(periodicStatesInfo);

25454
          rt_FREE(statesInfo2);

25455
        }

25456


25457
        {

25458
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25459
          rt_FREE(ptr);

25460
        }

25461


25462
        {

25463
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25464
          rt_FREE(ptr);

25465
        }

25466


25467
        {

25468
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25469
          rt_FREE(ptr);

25470
        }

25471


25472
        {

25473
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

25474
          rt_FREE(ptr);

25475
        }

25476


25477
        {

25478
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

25479
          rt_FREE(ptr);

25480
        }

25481


25482
        {

25483
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

25484
          rt_FREE(ptr);

25485
        }

25486


25487
        rt_FREE(ssGetPortInfoForInputs(rts));

25488
        rt_FREE(ssGetPortInfoForOutputs(rts));

25489
        rt_FREE(ssGetSFcnDWork(rts));

25490
      }

25491
    }

25492


25493
    /* RTW Generated Level2 S-Function Block: '<S125>/Generated S-Function6' (Res180_sf) */

25494
    {

25495
      SimStruct *rts = ssGetSFunction(S, 51);

25496
      if (rts != (NULL) ) {

25497
        rt_FREE(ssGetSampleTimePtr(rts));

25498
        rt_FREE(ssGetOffsetTimePtr(rts));

25499
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25500


25501
        /* Destroy model methods 3 */

25502
        {

25503
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25504
          rt_FREE(methods3);

25505
        }

25506


25507
        /* Destroy model methods 2 */

25508
        {

25509
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25510
          rt_FREE(methods2);

25511
        }

25512


25513
        /* Destroy state auxilliary information */

25514
        {

25515
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25516
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25517
          rt_FREE(periodicStatesInfo);

25518
          rt_FREE(statesInfo2);

25519
        }

25520


25521
        {

25522
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25523
          rt_FREE(ptr);

25524
        }

25525


25526
        rt_FREE(ssGetPortInfoForInputs(rts));

25527
        rt_FREE(ssGetPortInfoForOutputs(rts));

25528
      }

25529
    }

25530


25531
    /* RTW Generated Level2 S-Function Block: '<S134>/Generated S-Function1' (SaturationLimiter_sf) */

25532
    {

25533
      SimStruct *rts = ssGetSFunction(S, 52);

25534
      if (rts != (NULL) ) {

25535
        rt_FREE(ssGetSampleTimePtr(rts));

25536
        rt_FREE(ssGetOffsetTimePtr(rts));

25537
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25538


25539
        /* Destroy model methods 3 */

25540
        {

25541
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25542
          rt_FREE(methods3);

25543
        }

25544


25545
        /* Destroy model methods 2 */

25546
        {

25547
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25548
          rt_FREE(methods2);

25549
        }

25550


25551
        /* Destroy state auxilliary information */

25552
        {

25553
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25554
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25555
          rt_FREE(periodicStatesInfo);

25556
          rt_FREE(statesInfo2);

25557
        }

25558


25559
        {

25560
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25561
          rt_FREE(ptr);

25562
        }

25563


25564
        {

25565
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25566
          rt_FREE(ptr);

25567
        }

25568


25569
        {

25570
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25571
          rt_FREE(ptr);

25572
        }

25573


25574
        rt_FREE(ssGetPortInfoForInputs(rts));

25575
        rt_FREE(ssGetPortInfoForOutputs(rts));

25576
      }

25577
    }

25578


25579
    /* RTW Generated Level2 S-Function Block: '<S135>/Generated S-Function' (Gain_sf) */

25580
    {

25581
      SimStruct *rts = ssGetSFunction(S, 53);

25582
      if (rts != (NULL) ) {

25583
        rt_FREE(ssGetSampleTimePtr(rts));

25584
        rt_FREE(ssGetOffsetTimePtr(rts));

25585
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25586


25587
        /* Destroy model methods 3 */

25588
        {

25589
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25590
          rt_FREE(methods3);

25591
        }

25592


25593
        /* Destroy model methods 2 */

25594
        {

25595
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25596
          rt_FREE(methods2);

25597
        }

25598


25599
        /* Destroy state auxilliary information */

25600
        {

25601
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25602
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25603
          rt_FREE(periodicStatesInfo);

25604
          rt_FREE(statesInfo2);

25605
        }

25606


25607
        {

25608
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25609
          rt_FREE(ptr);

25610
        }

25611


25612
        {

25613
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25614
          rt_FREE(ptr);

25615
        }

25616


25617
        rt_FREE(ssGetPortInfoForInputs(rts));

25618
        rt_FREE(ssGetPortInfoForOutputs(rts));

25619
      }

25620
    }

25621


25622
    /* RTW Generated Level2 S-Function Block: '<S136>/Generated S-Function' (Gain_sf) */

25623
    {

25624
      SimStruct *rts = ssGetSFunction(S, 54);

25625
      if (rts != (NULL) ) {

25626
        rt_FREE(ssGetSampleTimePtr(rts));

25627
        rt_FREE(ssGetOffsetTimePtr(rts));

25628
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25629


25630
        /* Destroy model methods 3 */

25631
        {

25632
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25633
          rt_FREE(methods3);

25634
        }

25635


25636
        /* Destroy model methods 2 */

25637
        {

25638
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25639
          rt_FREE(methods2);

25640
        }

25641


25642
        /* Destroy state auxilliary information */

25643
        {

25644
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25645
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25646
          rt_FREE(periodicStatesInfo);

25647
          rt_FREE(statesInfo2);

25648
        }

25649


25650
        {

25651
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25652
          rt_FREE(ptr);

25653
        }

25654


25655
        {

25656
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25657
          rt_FREE(ptr);

25658
        }

25659


25660
        rt_FREE(ssGetPortInfoForInputs(rts));

25661
        rt_FREE(ssGetPortInfoForOutputs(rts));

25662
      }

25663
    }

25664


25665
    /* RTW Generated Level2 S-Function Block: '<S137>/Generated S-Function' (Gain_sf) */

25666
    {

25667
      SimStruct *rts = ssGetSFunction(S, 55);

25668
      if (rts != (NULL) ) {

25669
        rt_FREE(ssGetSampleTimePtr(rts));

25670
        rt_FREE(ssGetOffsetTimePtr(rts));

25671
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25672


25673
        /* Destroy model methods 3 */

25674
        {

25675
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25676
          rt_FREE(methods3);

25677
        }

25678


25679
        /* Destroy model methods 2 */

25680
        {

25681
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25682
          rt_FREE(methods2);

25683
        }

25684


25685
        /* Destroy state auxilliary information */

25686
        {

25687
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25688
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25689
          rt_FREE(periodicStatesInfo);

25690
          rt_FREE(statesInfo2);

25691
        }

25692


25693
        {

25694
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25695
          rt_FREE(ptr);

25696
        }

25697


25698
        {

25699
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25700
          rt_FREE(ptr);

25701
        }

25702


25703
        rt_FREE(ssGetPortInfoForInputs(rts));

25704
        rt_FREE(ssGetPortInfoForOutputs(rts));

25705
      }

25706
    }

25707


25708
    /* RTW Generated Level2 S-Function Block: '<S132>/Generated S-Function3' (IntegratorLimited_sf) */

25709
    {

25710
      SimStruct *rts = ssGetSFunction(S, 56);

25711
      if (rts != (NULL) ) {

25712
        rt_FREE(ssGetSampleTimePtr(rts));

25713
        rt_FREE(ssGetOffsetTimePtr(rts));

25714
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25715


25716
        /* Destroy model methods 3 */

25717
        {

25718
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25719
          rt_FREE(methods3);

25720
        }

25721


25722
        /* Destroy model methods 2 */

25723
        {

25724
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25725
          rt_FREE(methods2);

25726
        }

25727


25728
        /* Destroy state auxilliary information */

25729
        {

25730
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25731
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25732
          rt_FREE(periodicStatesInfo);

25733
          rt_FREE(statesInfo2);

25734
        }

25735


25736
        {

25737
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25738
          rt_FREE(ptr);

25739
        }

25740


25741
        {

25742
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25743
          rt_FREE(ptr);

25744
        }

25745


25746
        {

25747
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25748
          rt_FREE(ptr);

25749
        }

25750


25751
        {

25752
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

25753
          rt_FREE(ptr);

25754
        }

25755


25756
        {

25757
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

25758
          rt_FREE(ptr);

25759
        }

25760


25761
        {

25762
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

25763
          rt_FREE(ptr);

25764
        }

25765


25766
        {

25767
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

25768
          rt_FREE(ptr);

25769
        }

25770


25771
        rt_FREE(ssGetPortInfoForInputs(rts));

25772
        rt_FREE(ssGetPortInfoForOutputs(rts));

25773
        rt_FREE(ssGetSFcnDWork(rts));

25774
      }

25775
    }

25776


25777
    /* RTW Generated Level2 S-Function Block: '<S133>/Generated S-Function2' (LeadLagFilter_sf) */

25778
    {

25779
      SimStruct *rts = ssGetSFunction(S, 57);

25780
      if (rts != (NULL) ) {

25781
        rt_FREE(ssGetSampleTimePtr(rts));

25782
        rt_FREE(ssGetOffsetTimePtr(rts));

25783
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25784


25785
        /* Destroy model methods 3 */

25786
        {

25787
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25788
          rt_FREE(methods3);

25789
        }

25790


25791
        /* Destroy model methods 2 */

25792
        {

25793
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25794
          rt_FREE(methods2);

25795
        }

25796


25797
        /* Destroy state auxilliary information */

25798
        {

25799
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25800
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25801
          rt_FREE(periodicStatesInfo);

25802
          rt_FREE(statesInfo2);

25803
        }

25804


25805
        {

25806
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25807
          rt_FREE(ptr);

25808
        }

25809


25810
        {

25811
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25812
          rt_FREE(ptr);

25813
        }

25814


25815
        {

25816
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25817
          rt_FREE(ptr);

25818
        }

25819


25820
        {

25821
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

25822
          rt_FREE(ptr);

25823
        }

25824


25825
        {

25826
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

25827
          rt_FREE(ptr);

25828
        }

25829


25830
        {

25831
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

25832
          rt_FREE(ptr);

25833
        }

25834


25835
        {

25836
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

25837
          rt_FREE(ptr);

25838
        }

25839


25840
        rt_FREE(ssGetPortInfoForInputs(rts));

25841
        rt_FREE(ssGetPortInfoForOutputs(rts));

25842
        rt_FREE(ssGetSFcnDWork(rts));

25843
      }

25844
    }

25845


25846
    /* RTW Generated Level2 S-Function Block: '<S131>/Generated S-Function4' (LagFilter_sf) */

25847
    {

25848
      SimStruct *rts = ssGetSFunction(S, 58);

25849
      if (rts != (NULL) ) {

25850
        rt_FREE(ssGetSampleTimePtr(rts));

25851
        rt_FREE(ssGetOffsetTimePtr(rts));

25852
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25853


25854
        /* Destroy model methods 3 */

25855
        {

25856
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25857
          rt_FREE(methods3);

25858
        }

25859


25860
        /* Destroy model methods 2 */

25861
        {

25862
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25863
          rt_FREE(methods2);

25864
        }

25865


25866
        /* Destroy state auxilliary information */

25867
        {

25868
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25869
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25870
          rt_FREE(periodicStatesInfo);

25871
          rt_FREE(statesInfo2);

25872
        }

25873


25874
        {

25875
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25876
          rt_FREE(ptr);

25877
        }

25878


25879
        {

25880
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25881
          rt_FREE(ptr);

25882
        }

25883


25884
        {

25885
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25886
          rt_FREE(ptr);

25887
        }

25888


25889
        {

25890
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

25891
          rt_FREE(ptr);

25892
        }

25893


25894
        {

25895
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

25896
          rt_FREE(ptr);

25897
        }

25898


25899
        {

25900
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

25901
          rt_FREE(ptr);

25902
        }

25903


25904
        rt_FREE(ssGetPortInfoForInputs(rts));

25905
        rt_FREE(ssGetPortInfoForOutputs(rts));

25906
        rt_FREE(ssGetSFcnDWork(rts));

25907
      }

25908
    }

25909


25910
    /* RTW Generated Level2 S-Function Block: '<S155>/Generated S-Function1' (SaturationLimiter_sf) */

25911
    {

25912
      SimStruct *rts = ssGetSFunction(S, 59);

25913
      if (rts != (NULL) ) {

25914
        rt_FREE(ssGetSampleTimePtr(rts));

25915
        rt_FREE(ssGetOffsetTimePtr(rts));

25916
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25917


25918
        /* Destroy model methods 3 */

25919
        {

25920
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25921
          rt_FREE(methods3);

25922
        }

25923


25924
        /* Destroy model methods 2 */

25925
        {

25926
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25927
          rt_FREE(methods2);

25928
        }

25929


25930
        /* Destroy state auxilliary information */

25931
        {

25932
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25933
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25934
          rt_FREE(periodicStatesInfo);

25935
          rt_FREE(statesInfo2);

25936
        }

25937


25938
        {

25939
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25940
          rt_FREE(ptr);

25941
        }

25942


25943
        {

25944
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

25945
          rt_FREE(ptr);

25946
        }

25947


25948
        {

25949
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

25950
          rt_FREE(ptr);

25951
        }

25952


25953
        rt_FREE(ssGetPortInfoForInputs(rts));

25954
        rt_FREE(ssGetPortInfoForOutputs(rts));

25955
      }

25956
    }

25957


25958
    /* RTW Generated Level2 S-Function Block: '<S128>/Generated S-Function6' (Res180_sf) */

25959
    {

25960
      SimStruct *rts = ssGetSFunction(S, 60);

25961
      if (rts != (NULL) ) {

25962
        rt_FREE(ssGetSampleTimePtr(rts));

25963
        rt_FREE(ssGetOffsetTimePtr(rts));

25964
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

25965


25966
        /* Destroy model methods 3 */

25967
        {

25968
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

25969
          rt_FREE(methods3);

25970
        }

25971


25972
        /* Destroy model methods 2 */

25973
        {

25974
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

25975
          rt_FREE(methods2);

25976
        }

25977


25978
        /* Destroy state auxilliary information */

25979
        {

25980
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

25981
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

25982
          rt_FREE(periodicStatesInfo);

25983
          rt_FREE(statesInfo2);

25984
        }

25985


25986
        {

25987
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

25988
          rt_FREE(ptr);

25989
        }

25990


25991
        rt_FREE(ssGetPortInfoForInputs(rts));

25992
        rt_FREE(ssGetPortInfoForOutputs(rts));

25993
      }

25994
    }

25995


25996
    /* RTW Generated Level2 S-Function Block: '<S154>/Generated S-Function1' (SaturationLimiter_sf) */

25997
    {

25998
      SimStruct *rts = ssGetSFunction(S, 61);

25999
      if (rts != (NULL) ) {

26000
        rt_FREE(ssGetSampleTimePtr(rts));

26001
        rt_FREE(ssGetOffsetTimePtr(rts));

26002
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26003


26004
        /* Destroy model methods 3 */

26005
        {

26006
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26007
          rt_FREE(methods3);

26008
        }

26009


26010
        /* Destroy model methods 2 */

26011
        {

26012
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26013
          rt_FREE(methods2);

26014
        }

26015


26016
        /* Destroy state auxilliary information */

26017
        {

26018
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26019
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26020
          rt_FREE(periodicStatesInfo);

26021
          rt_FREE(statesInfo2);

26022
        }

26023


26024
        {

26025
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26026
          rt_FREE(ptr);

26027
        }

26028


26029
        {

26030
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26031
          rt_FREE(ptr);

26032
        }

26033


26034
        {

26035
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26036
          rt_FREE(ptr);

26037
        }

26038


26039
        rt_FREE(ssGetPortInfoForInputs(rts));

26040
        rt_FREE(ssGetPortInfoForOutputs(rts));

26041
      }

26042
    }

26043


26044
    /* RTW Generated Level2 S-Function Block: '<S158>/Generated S-Function' (Gain_sf) */

26045
    {

26046
      SimStruct *rts = ssGetSFunction(S, 62);

26047
      if (rts != (NULL) ) {

26048
        rt_FREE(ssGetSampleTimePtr(rts));

26049
        rt_FREE(ssGetOffsetTimePtr(rts));

26050
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26051


26052
        /* Destroy model methods 3 */

26053
        {

26054
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26055
          rt_FREE(methods3);

26056
        }

26057


26058
        /* Destroy model methods 2 */

26059
        {

26060
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26061
          rt_FREE(methods2);

26062
        }

26063


26064
        /* Destroy state auxilliary information */

26065
        {

26066
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26067
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26068
          rt_FREE(periodicStatesInfo);

26069
          rt_FREE(statesInfo2);

26070
        }

26071


26072
        {

26073
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26074
          rt_FREE(ptr);

26075
        }

26076


26077
        {

26078
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26079
          rt_FREE(ptr);

26080
        }

26081


26082
        rt_FREE(ssGetPortInfoForInputs(rts));

26083
        rt_FREE(ssGetPortInfoForOutputs(rts));

26084
      }

26085
    }

26086


26087
    /* RTW Generated Level2 S-Function Block: '<S152>/Generated S-Function2' (LeadLagFilter_sf) */

26088
    {

26089
      SimStruct *rts = ssGetSFunction(S, 63);

26090
      if (rts != (NULL) ) {

26091
        rt_FREE(ssGetSampleTimePtr(rts));

26092
        rt_FREE(ssGetOffsetTimePtr(rts));

26093
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26094


26095
        /* Destroy model methods 3 */

26096
        {

26097
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26098
          rt_FREE(methods3);

26099
        }

26100


26101
        /* Destroy model methods 2 */

26102
        {

26103
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26104
          rt_FREE(methods2);

26105
        }

26106


26107
        /* Destroy state auxilliary information */

26108
        {

26109
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26110
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26111
          rt_FREE(periodicStatesInfo);

26112
          rt_FREE(statesInfo2);

26113
        }

26114


26115
        {

26116
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26117
          rt_FREE(ptr);

26118
        }

26119


26120
        {

26121
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26122
          rt_FREE(ptr);

26123
        }

26124


26125
        {

26126
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26127
          rt_FREE(ptr);

26128
        }

26129


26130
        {

26131
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

26132
          rt_FREE(ptr);

26133
        }

26134


26135
        {

26136
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

26137
          rt_FREE(ptr);

26138
        }

26139


26140
        {

26141
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

26142
          rt_FREE(ptr);

26143
        }

26144


26145
        {

26146
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

26147
          rt_FREE(ptr);

26148
        }

26149


26150
        rt_FREE(ssGetPortInfoForInputs(rts));

26151
        rt_FREE(ssGetPortInfoForOutputs(rts));

26152
        rt_FREE(ssGetSFcnDWork(rts));

26153
      }

26154
    }

26155


26156
    /* RTW Generated Level2 S-Function Block: '<S128>/Generated S-Function1' (Res180_sf) */

26157
    {

26158
      SimStruct *rts = ssGetSFunction(S, 64);

26159
      if (rts != (NULL) ) {

26160
        rt_FREE(ssGetSampleTimePtr(rts));

26161
        rt_FREE(ssGetOffsetTimePtr(rts));

26162
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26163


26164
        /* Destroy model methods 3 */

26165
        {

26166
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26167
          rt_FREE(methods3);

26168
        }

26169


26170
        /* Destroy model methods 2 */

26171
        {

26172
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26173
          rt_FREE(methods2);

26174
        }

26175


26176
        /* Destroy state auxilliary information */

26177
        {

26178
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26179
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26180
          rt_FREE(periodicStatesInfo);

26181
          rt_FREE(statesInfo2);

26182
        }

26183


26184
        {

26185
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26186
          rt_FREE(ptr);

26187
        }

26188


26189
        rt_FREE(ssGetPortInfoForInputs(rts));

26190
        rt_FREE(ssGetPortInfoForOutputs(rts));

26191
      }

26192
    }

26193


26194
    /* RTW Generated Level2 S-Function Block: '<S153>/Generated S-Function1' (SaturationLimiter_sf) */

26195
    {

26196
      SimStruct *rts = ssGetSFunction(S, 65);

26197
      if (rts != (NULL) ) {

26198
        rt_FREE(ssGetSampleTimePtr(rts));

26199
        rt_FREE(ssGetOffsetTimePtr(rts));

26200
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26201


26202
        /* Destroy model methods 3 */

26203
        {

26204
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26205
          rt_FREE(methods3);

26206
        }

26207


26208
        /* Destroy model methods 2 */

26209
        {

26210
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26211
          rt_FREE(methods2);

26212
        }

26213


26214
        /* Destroy state auxilliary information */

26215
        {

26216
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26217
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26218
          rt_FREE(periodicStatesInfo);

26219
          rt_FREE(statesInfo2);

26220
        }

26221


26222
        {

26223
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26224
          rt_FREE(ptr);

26225
        }

26226


26227
        {

26228
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26229
          rt_FREE(ptr);

26230
        }

26231


26232
        {

26233
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26234
          rt_FREE(ptr);

26235
        }

26236


26237
        rt_FREE(ssGetPortInfoForInputs(rts));

26238
        rt_FREE(ssGetPortInfoForOutputs(rts));

26239
      }

26240
    }

26241


26242
    /* RTW Generated Level2 S-Function Block: '<S156>/Generated S-Function' (Gain_sf) */

26243
    {

26244
      SimStruct *rts = ssGetSFunction(S, 66);

26245
      if (rts != (NULL) ) {

26246
        rt_FREE(ssGetSampleTimePtr(rts));

26247
        rt_FREE(ssGetOffsetTimePtr(rts));

26248
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26249


26250
        /* Destroy model methods 3 */

26251
        {

26252
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26253
          rt_FREE(methods3);

26254
        }

26255


26256
        /* Destroy model methods 2 */

26257
        {

26258
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26259
          rt_FREE(methods2);

26260
        }

26261


26262
        /* Destroy state auxilliary information */

26263
        {

26264
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26265
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26266
          rt_FREE(periodicStatesInfo);

26267
          rt_FREE(statesInfo2);

26268
        }

26269


26270
        {

26271
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26272
          rt_FREE(ptr);

26273
        }

26274


26275
        {

26276
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26277
          rt_FREE(ptr);

26278
        }

26279


26280
        rt_FREE(ssGetPortInfoForInputs(rts));

26281
        rt_FREE(ssGetPortInfoForOutputs(rts));

26282
      }

26283
    }

26284


26285
    /* RTW Generated Level2 S-Function Block: '<S157>/Generated S-Function' (Gain_sf) */

26286
    {

26287
      SimStruct *rts = ssGetSFunction(S, 67);

26288
      if (rts != (NULL) ) {

26289
        rt_FREE(ssGetSampleTimePtr(rts));

26290
        rt_FREE(ssGetOffsetTimePtr(rts));

26291
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26292


26293
        /* Destroy model methods 3 */

26294
        {

26295
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26296
          rt_FREE(methods3);

26297
        }

26298


26299
        /* Destroy model methods 2 */

26300
        {

26301
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26302
          rt_FREE(methods2);

26303
        }

26304


26305
        /* Destroy state auxilliary information */

26306
        {

26307
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26308
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26309
          rt_FREE(periodicStatesInfo);

26310
          rt_FREE(statesInfo2);

26311
        }

26312


26313
        {

26314
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26315
          rt_FREE(ptr);

26316
        }

26317


26318
        {

26319
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26320
          rt_FREE(ptr);

26321
        }

26322


26323
        rt_FREE(ssGetPortInfoForInputs(rts));

26324
        rt_FREE(ssGetPortInfoForOutputs(rts));

26325
      }

26326
    }

26327


26328
    /* RTW Generated Level2 S-Function Block: '<S150>/Generated S-Function3' (IntegratorLimited_sf) */

26329
    {

26330
      SimStruct *rts = ssGetSFunction(S, 68);

26331
      if (rts != (NULL) ) {

26332
        rt_FREE(ssGetSampleTimePtr(rts));

26333
        rt_FREE(ssGetOffsetTimePtr(rts));

26334
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26335


26336
        /* Destroy model methods 3 */

26337
        {

26338
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26339
          rt_FREE(methods3);

26340
        }

26341


26342
        /* Destroy model methods 2 */

26343
        {

26344
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26345
          rt_FREE(methods2);

26346
        }

26347


26348
        /* Destroy state auxilliary information */

26349
        {

26350
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26351
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26352
          rt_FREE(periodicStatesInfo);

26353
          rt_FREE(statesInfo2);

26354
        }

26355


26356
        {

26357
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26358
          rt_FREE(ptr);

26359
        }

26360


26361
        {

26362
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26363
          rt_FREE(ptr);

26364
        }

26365


26366
        {

26367
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26368
          rt_FREE(ptr);

26369
        }

26370


26371
        {

26372
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

26373
          rt_FREE(ptr);

26374
        }

26375


26376
        {

26377
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

26378
          rt_FREE(ptr);

26379
        }

26380


26381
        {

26382
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

26383
          rt_FREE(ptr);

26384
        }

26385


26386
        {

26387
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

26388
          rt_FREE(ptr);

26389
        }

26390


26391
        rt_FREE(ssGetPortInfoForInputs(rts));

26392
        rt_FREE(ssGetPortInfoForOutputs(rts));

26393
        rt_FREE(ssGetSFcnDWork(rts));

26394
      }

26395
    }

26396


26397
    /* RTW Generated Level2 S-Function Block: '<S151>/Generated S-Function2' (LeadLagFilter_sf) */

26398
    {

26399
      SimStruct *rts = ssGetSFunction(S, 69);

26400
      if (rts != (NULL) ) {

26401
        rt_FREE(ssGetSampleTimePtr(rts));

26402
        rt_FREE(ssGetOffsetTimePtr(rts));

26403
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26404


26405
        /* Destroy model methods 3 */

26406
        {

26407
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26408
          rt_FREE(methods3);

26409
        }

26410


26411
        /* Destroy model methods 2 */

26412
        {

26413
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26414
          rt_FREE(methods2);

26415
        }

26416


26417
        /* Destroy state auxilliary information */

26418
        {

26419
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26420
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26421
          rt_FREE(periodicStatesInfo);

26422
          rt_FREE(statesInfo2);

26423
        }

26424


26425
        {

26426
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26427
          rt_FREE(ptr);

26428
        }

26429


26430
        {

26431
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26432
          rt_FREE(ptr);

26433
        }

26434


26435
        {

26436
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26437
          rt_FREE(ptr);

26438
        }

26439


26440
        {

26441
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

26442
          rt_FREE(ptr);

26443
        }

26444


26445
        {

26446
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

26447
          rt_FREE(ptr);

26448
        }

26449


26450
        {

26451
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

26452
          rt_FREE(ptr);

26453
        }

26454


26455
        {

26456
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

26457
          rt_FREE(ptr);

26458
        }

26459


26460
        rt_FREE(ssGetPortInfoForInputs(rts));

26461
        rt_FREE(ssGetPortInfoForOutputs(rts));

26462
        rt_FREE(ssGetSFcnDWork(rts));

26463
      }

26464
    }

26465


26466
    /* RTW Generated Level2 S-Function Block: '<S149>/Generated S-Function4' (LagFilter_sf) */

26467
    {

26468
      SimStruct *rts = ssGetSFunction(S, 70);

26469
      if (rts != (NULL) ) {

26470
        rt_FREE(ssGetSampleTimePtr(rts));

26471
        rt_FREE(ssGetOffsetTimePtr(rts));

26472
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26473


26474
        /* Destroy model methods 3 */

26475
        {

26476
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26477
          rt_FREE(methods3);

26478
        }

26479


26480
        /* Destroy model methods 2 */

26481
        {

26482
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26483
          rt_FREE(methods2);

26484
        }

26485


26486
        /* Destroy state auxilliary information */

26487
        {

26488
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26489
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26490
          rt_FREE(periodicStatesInfo);

26491
          rt_FREE(statesInfo2);

26492
        }

26493


26494
        {

26495
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26496
          rt_FREE(ptr);

26497
        }

26498


26499
        {

26500
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26501
          rt_FREE(ptr);

26502
        }

26503


26504
        {

26505
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26506
          rt_FREE(ptr);

26507
        }

26508


26509
        {

26510
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

26511
          rt_FREE(ptr);

26512
        }

26513


26514
        {

26515
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

26516
          rt_FREE(ptr);

26517
        }

26518


26519
        {

26520
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

26521
          rt_FREE(ptr);

26522
        }

26523


26524
        rt_FREE(ssGetPortInfoForInputs(rts));

26525
        rt_FREE(ssGetPortInfoForOutputs(rts));

26526
        rt_FREE(ssGetSFcnDWork(rts));

26527
      }

26528
    }

26529


26530
    /* RTW Generated Level2 S-Function Block: '<S145>/Generated S-Function1' (SaturationLimiter_sf) */

26531
    {

26532
      SimStruct *rts = ssGetSFunction(S, 71);

26533
      if (rts != (NULL) ) {

26534
        rt_FREE(ssGetSampleTimePtr(rts));

26535
        rt_FREE(ssGetOffsetTimePtr(rts));

26536
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26537


26538
        /* Destroy model methods 3 */

26539
        {

26540
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26541
          rt_FREE(methods3);

26542
        }

26543


26544
        /* Destroy model methods 2 */

26545
        {

26546
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26547
          rt_FREE(methods2);

26548
        }

26549


26550
        /* Destroy state auxilliary information */

26551
        {

26552
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26553
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26554
          rt_FREE(periodicStatesInfo);

26555
          rt_FREE(statesInfo2);

26556
        }

26557


26558
        {

26559
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26560
          rt_FREE(ptr);

26561
        }

26562


26563
        {

26564
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26565
          rt_FREE(ptr);

26566
        }

26567


26568
        {

26569
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26570
          rt_FREE(ptr);

26571
        }

26572


26573
        rt_FREE(ssGetPortInfoForInputs(rts));

26574
        rt_FREE(ssGetPortInfoForOutputs(rts));

26575
      }

26576
    }

26577


26578
    /* RTW Generated Level2 S-Function Block: '<S127>/Generated S-Function6' (Res180_sf) */

26579
    {

26580
      SimStruct *rts = ssGetSFunction(S, 72);

26581
      if (rts != (NULL) ) {

26582
        rt_FREE(ssGetSampleTimePtr(rts));

26583
        rt_FREE(ssGetOffsetTimePtr(rts));

26584
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26585


26586
        /* Destroy model methods 3 */

26587
        {

26588
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26589
          rt_FREE(methods3);

26590
        }

26591


26592
        /* Destroy model methods 2 */

26593
        {

26594
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26595
          rt_FREE(methods2);

26596
        }

26597


26598
        /* Destroy state auxilliary information */

26599
        {

26600
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26601
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26602
          rt_FREE(periodicStatesInfo);

26603
          rt_FREE(statesInfo2);

26604
        }

26605


26606
        {

26607
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26608
          rt_FREE(ptr);

26609
        }

26610


26611
        rt_FREE(ssGetPortInfoForInputs(rts));

26612
        rt_FREE(ssGetPortInfoForOutputs(rts));

26613
      }

26614
    }

26615


26616
    /* RTW Generated Level2 S-Function Block: '<S144>/Generated S-Function1' (SaturationLimiter_sf) */

26617
    {

26618
      SimStruct *rts = ssGetSFunction(S, 73);

26619
      if (rts != (NULL) ) {

26620
        rt_FREE(ssGetSampleTimePtr(rts));

26621
        rt_FREE(ssGetOffsetTimePtr(rts));

26622
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26623


26624
        /* Destroy model methods 3 */

26625
        {

26626
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26627
          rt_FREE(methods3);

26628
        }

26629


26630
        /* Destroy model methods 2 */

26631
        {

26632
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26633
          rt_FREE(methods2);

26634
        }

26635


26636
        /* Destroy state auxilliary information */

26637
        {

26638
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26639
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26640
          rt_FREE(periodicStatesInfo);

26641
          rt_FREE(statesInfo2);

26642
        }

26643


26644
        {

26645
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26646
          rt_FREE(ptr);

26647
        }

26648


26649
        {

26650
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26651
          rt_FREE(ptr);

26652
        }

26653


26654
        {

26655
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26656
          rt_FREE(ptr);

26657
        }

26658


26659
        rt_FREE(ssGetPortInfoForInputs(rts));

26660
        rt_FREE(ssGetPortInfoForOutputs(rts));

26661
      }

26662
    }

26663


26664
    /* RTW Generated Level2 S-Function Block: '<S148>/Generated S-Function' (Gain_sf) */

26665
    {

26666
      SimStruct *rts = ssGetSFunction(S, 74);

26667
      if (rts != (NULL) ) {

26668
        rt_FREE(ssGetSampleTimePtr(rts));

26669
        rt_FREE(ssGetOffsetTimePtr(rts));

26670
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26671


26672
        /* Destroy model methods 3 */

26673
        {

26674
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26675
          rt_FREE(methods3);

26676
        }

26677


26678
        /* Destroy model methods 2 */

26679
        {

26680
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26681
          rt_FREE(methods2);

26682
        }

26683


26684
        /* Destroy state auxilliary information */

26685
        {

26686
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26687
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26688
          rt_FREE(periodicStatesInfo);

26689
          rt_FREE(statesInfo2);

26690
        }

26691


26692
        {

26693
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26694
          rt_FREE(ptr);

26695
        }

26696


26697
        {

26698
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26699
          rt_FREE(ptr);

26700
        }

26701


26702
        rt_FREE(ssGetPortInfoForInputs(rts));

26703
        rt_FREE(ssGetPortInfoForOutputs(rts));

26704
      }

26705
    }

26706


26707
    /* RTW Generated Level2 S-Function Block: '<S142>/Generated S-Function2' (LeadLagFilter_sf) */

26708
    {

26709
      SimStruct *rts = ssGetSFunction(S, 75);

26710
      if (rts != (NULL) ) {

26711
        rt_FREE(ssGetSampleTimePtr(rts));

26712
        rt_FREE(ssGetOffsetTimePtr(rts));

26713
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26714


26715
        /* Destroy model methods 3 */

26716
        {

26717
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26718
          rt_FREE(methods3);

26719
        }

26720


26721
        /* Destroy model methods 2 */

26722
        {

26723
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26724
          rt_FREE(methods2);

26725
        }

26726


26727
        /* Destroy state auxilliary information */

26728
        {

26729
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26730
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26731
          rt_FREE(periodicStatesInfo);

26732
          rt_FREE(statesInfo2);

26733
        }

26734


26735
        {

26736
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26737
          rt_FREE(ptr);

26738
        }

26739


26740
        {

26741
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26742
          rt_FREE(ptr);

26743
        }

26744


26745
        {

26746
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26747
          rt_FREE(ptr);

26748
        }

26749


26750
        {

26751
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

26752
          rt_FREE(ptr);

26753
        }

26754


26755
        {

26756
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

26757
          rt_FREE(ptr);

26758
        }

26759


26760
        {

26761
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

26762
          rt_FREE(ptr);

26763
        }

26764


26765
        {

26766
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

26767
          rt_FREE(ptr);

26768
        }

26769


26770
        rt_FREE(ssGetPortInfoForInputs(rts));

26771
        rt_FREE(ssGetPortInfoForOutputs(rts));

26772
        rt_FREE(ssGetSFcnDWork(rts));

26773
      }

26774
    }

26775


26776
    /* RTW Generated Level2 S-Function Block: '<S143>/Generated S-Function1' (SaturationLimiter_sf) */

26777
    {

26778
      SimStruct *rts = ssGetSFunction(S, 76);

26779
      if (rts != (NULL) ) {

26780
        rt_FREE(ssGetSampleTimePtr(rts));

26781
        rt_FREE(ssGetOffsetTimePtr(rts));

26782
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26783


26784
        /* Destroy model methods 3 */

26785
        {

26786
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26787
          rt_FREE(methods3);

26788
        }

26789


26790
        /* Destroy model methods 2 */

26791
        {

26792
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26793
          rt_FREE(methods2);

26794
        }

26795


26796
        /* Destroy state auxilliary information */

26797
        {

26798
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26799
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26800
          rt_FREE(periodicStatesInfo);

26801
          rt_FREE(statesInfo2);

26802
        }

26803


26804
        {

26805
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26806
          rt_FREE(ptr);

26807
        }

26808


26809
        {

26810
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26811
          rt_FREE(ptr);

26812
        }

26813


26814
        {

26815
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26816
          rt_FREE(ptr);

26817
        }

26818


26819
        rt_FREE(ssGetPortInfoForInputs(rts));

26820
        rt_FREE(ssGetPortInfoForOutputs(rts));

26821
      }

26822
    }

26823


26824
    /* RTW Generated Level2 S-Function Block: '<S127>/Generated S-Function1' (Res180_sf) */

26825
    {

26826
      SimStruct *rts = ssGetSFunction(S, 77);

26827
      if (rts != (NULL) ) {

26828
        rt_FREE(ssGetSampleTimePtr(rts));

26829
        rt_FREE(ssGetOffsetTimePtr(rts));

26830
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26831


26832
        /* Destroy model methods 3 */

26833
        {

26834
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26835
          rt_FREE(methods3);

26836
        }

26837


26838
        /* Destroy model methods 2 */

26839
        {

26840
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26841
          rt_FREE(methods2);

26842
        }

26843


26844
        /* Destroy state auxilliary information */

26845
        {

26846
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26847
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26848
          rt_FREE(periodicStatesInfo);

26849
          rt_FREE(statesInfo2);

26850
        }

26851


26852
        {

26853
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26854
          rt_FREE(ptr);

26855
        }

26856


26857
        rt_FREE(ssGetPortInfoForInputs(rts));

26858
        rt_FREE(ssGetPortInfoForOutputs(rts));

26859
      }

26860
    }

26861


26862
    /* RTW Generated Level2 S-Function Block: '<S146>/Generated S-Function' (Gain_sf) */

26863
    {

26864
      SimStruct *rts = ssGetSFunction(S, 78);

26865
      if (rts != (NULL) ) {

26866
        rt_FREE(ssGetSampleTimePtr(rts));

26867
        rt_FREE(ssGetOffsetTimePtr(rts));

26868
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26869


26870
        /* Destroy model methods 3 */

26871
        {

26872
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26873
          rt_FREE(methods3);

26874
        }

26875


26876
        /* Destroy model methods 2 */

26877
        {

26878
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26879
          rt_FREE(methods2);

26880
        }

26881


26882
        /* Destroy state auxilliary information */

26883
        {

26884
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26885
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26886
          rt_FREE(periodicStatesInfo);

26887
          rt_FREE(statesInfo2);

26888
        }

26889


26890
        {

26891
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26892
          rt_FREE(ptr);

26893
        }

26894


26895
        {

26896
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26897
          rt_FREE(ptr);

26898
        }

26899


26900
        rt_FREE(ssGetPortInfoForInputs(rts));

26901
        rt_FREE(ssGetPortInfoForOutputs(rts));

26902
      }

26903
    }

26904


26905
    /* RTW Generated Level2 S-Function Block: '<S147>/Generated S-Function' (Gain_sf) */

26906
    {

26907
      SimStruct *rts = ssGetSFunction(S, 79);

26908
      if (rts != (NULL) ) {

26909
        rt_FREE(ssGetSampleTimePtr(rts));

26910
        rt_FREE(ssGetOffsetTimePtr(rts));

26911
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26912


26913
        /* Destroy model methods 3 */

26914
        {

26915
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26916
          rt_FREE(methods3);

26917
        }

26918


26919
        /* Destroy model methods 2 */

26920
        {

26921
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26922
          rt_FREE(methods2);

26923
        }

26924


26925
        /* Destroy state auxilliary information */

26926
        {

26927
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26928
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26929
          rt_FREE(periodicStatesInfo);

26930
          rt_FREE(statesInfo2);

26931
        }

26932


26933
        {

26934
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26935
          rt_FREE(ptr);

26936
        }

26937


26938
        {

26939
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26940
          rt_FREE(ptr);

26941
        }

26942


26943
        rt_FREE(ssGetPortInfoForInputs(rts));

26944
        rt_FREE(ssGetPortInfoForOutputs(rts));

26945
      }

26946
    }

26947


26948
    /* RTW Generated Level2 S-Function Block: '<S140>/Generated S-Function3' (IntegratorLimited_sf) */

26949
    {

26950
      SimStruct *rts = ssGetSFunction(S, 80);

26951
      if (rts != (NULL) ) {

26952
        rt_FREE(ssGetSampleTimePtr(rts));

26953
        rt_FREE(ssGetOffsetTimePtr(rts));

26954
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

26955


26956
        /* Destroy model methods 3 */

26957
        {

26958
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

26959
          rt_FREE(methods3);

26960
        }

26961


26962
        /* Destroy model methods 2 */

26963
        {

26964
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

26965
          rt_FREE(methods2);

26966
        }

26967


26968
        /* Destroy state auxilliary information */

26969
        {

26970
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

26971
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

26972
          rt_FREE(periodicStatesInfo);

26973
          rt_FREE(statesInfo2);

26974
        }

26975


26976
        {

26977
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

26978
          rt_FREE(ptr);

26979
        }

26980


26981
        {

26982
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

26983
          rt_FREE(ptr);

26984
        }

26985


26986
        {

26987
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

26988
          rt_FREE(ptr);

26989
        }

26990


26991
        {

26992
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

26993
          rt_FREE(ptr);

26994
        }

26995


26996
        {

26997
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

26998
          rt_FREE(ptr);

26999
        }

27000


27001
        {

27002
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

27003
          rt_FREE(ptr);

27004
        }

27005


27006
        {

27007
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

27008
          rt_FREE(ptr);

27009
        }

27010


27011
        rt_FREE(ssGetPortInfoForInputs(rts));

27012
        rt_FREE(ssGetPortInfoForOutputs(rts));

27013
        rt_FREE(ssGetSFcnDWork(rts));

27014
      }

27015
    }

27016


27017
    /* RTW Generated Level2 S-Function Block: '<S141>/Generated S-Function2' (LeadLagFilter_sf) */

27018
    {

27019
      SimStruct *rts = ssGetSFunction(S, 81);

27020
      if (rts != (NULL) ) {

27021
        rt_FREE(ssGetSampleTimePtr(rts));

27022
        rt_FREE(ssGetOffsetTimePtr(rts));

27023
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27024


27025
        /* Destroy model methods 3 */

27026
        {

27027
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27028
          rt_FREE(methods3);

27029
        }

27030


27031
        /* Destroy model methods 2 */

27032
        {

27033
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27034
          rt_FREE(methods2);

27035
        }

27036


27037
        /* Destroy state auxilliary information */

27038
        {

27039
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27040
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27041
          rt_FREE(periodicStatesInfo);

27042
          rt_FREE(statesInfo2);

27043
        }

27044


27045
        {

27046
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27047
          rt_FREE(ptr);

27048
        }

27049


27050
        {

27051
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27052
          rt_FREE(ptr);

27053
        }

27054


27055
        {

27056
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27057
          rt_FREE(ptr);

27058
        }

27059


27060
        {

27061
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

27062
          rt_FREE(ptr);

27063
        }

27064


27065
        {

27066
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

27067
          rt_FREE(ptr);

27068
        }

27069


27070
        {

27071
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

27072
          rt_FREE(ptr);

27073
        }

27074


27075
        {

27076
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

27077
          rt_FREE(ptr);

27078
        }

27079


27080
        rt_FREE(ssGetPortInfoForInputs(rts));

27081
        rt_FREE(ssGetPortInfoForOutputs(rts));

27082
        rt_FREE(ssGetSFcnDWork(rts));

27083
      }

27084
    }

27085


27086
    /* RTW Generated Level2 S-Function Block: '<S139>/Generated S-Function4' (LagFilter_sf) */

27087
    {

27088
      SimStruct *rts = ssGetSFunction(S, 82);

27089
      if (rts != (NULL) ) {

27090
        rt_FREE(ssGetSampleTimePtr(rts));

27091
        rt_FREE(ssGetOffsetTimePtr(rts));

27092
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27093


27094
        /* Destroy model methods 3 */

27095
        {

27096
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27097
          rt_FREE(methods3);

27098
        }

27099


27100
        /* Destroy model methods 2 */

27101
        {

27102
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27103
          rt_FREE(methods2);

27104
        }

27105


27106
        /* Destroy state auxilliary information */

27107
        {

27108
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27109
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27110
          rt_FREE(periodicStatesInfo);

27111
          rt_FREE(statesInfo2);

27112
        }

27113


27114
        {

27115
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27116
          rt_FREE(ptr);

27117
        }

27118


27119
        {

27120
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27121
          rt_FREE(ptr);

27122
        }

27123


27124
        {

27125
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27126
          rt_FREE(ptr);

27127
        }

27128


27129
        {

27130
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

27131
          rt_FREE(ptr);

27132
        }

27133


27134
        {

27135
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

27136
          rt_FREE(ptr);

27137
        }

27138


27139
        {

27140
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

27141
          rt_FREE(ptr);

27142
        }

27143


27144
        rt_FREE(ssGetPortInfoForInputs(rts));

27145
        rt_FREE(ssGetPortInfoForOutputs(rts));

27146
        rt_FREE(ssGetSFcnDWork(rts));

27147
      }

27148
    }

27149


27150
    /* RTW Generated Level2 S-Function Block: '<S164>/Generated S-Function1' (SaturationLimiter_sf) */

27151
    {

27152
      SimStruct *rts = ssGetSFunction(S, 83);

27153
      if (rts != (NULL) ) {

27154
        rt_FREE(ssGetSampleTimePtr(rts));

27155
        rt_FREE(ssGetOffsetTimePtr(rts));

27156
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27157


27158
        /* Destroy model methods 3 */

27159
        {

27160
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27161
          rt_FREE(methods3);

27162
        }

27163


27164
        /* Destroy model methods 2 */

27165
        {

27166
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27167
          rt_FREE(methods2);

27168
        }

27169


27170
        /* Destroy state auxilliary information */

27171
        {

27172
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27173
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27174
          rt_FREE(periodicStatesInfo);

27175
          rt_FREE(statesInfo2);

27176
        }

27177


27178
        {

27179
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27180
          rt_FREE(ptr);

27181
        }

27182


27183
        {

27184
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27185
          rt_FREE(ptr);

27186
        }

27187


27188
        {

27189
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27190
          rt_FREE(ptr);

27191
        }

27192


27193
        rt_FREE(ssGetPortInfoForInputs(rts));

27194
        rt_FREE(ssGetPortInfoForOutputs(rts));

27195
      }

27196
    }

27197


27198
    /* RTW Generated Level2 S-Function Block: '<S168>/Generated S-Function' (Gain_sf) */

27199
    {

27200
      SimStruct *rts = ssGetSFunction(S, 84);

27201
      if (rts != (NULL) ) {

27202
        rt_FREE(ssGetSampleTimePtr(rts));

27203
        rt_FREE(ssGetOffsetTimePtr(rts));

27204
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27205


27206
        /* Destroy model methods 3 */

27207
        {

27208
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27209
          rt_FREE(methods3);

27210
        }

27211


27212
        /* Destroy model methods 2 */

27213
        {

27214
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27215
          rt_FREE(methods2);

27216
        }

27217


27218
        /* Destroy state auxilliary information */

27219
        {

27220
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27221
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27222
          rt_FREE(periodicStatesInfo);

27223
          rt_FREE(statesInfo2);

27224
        }

27225


27226
        {

27227
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27228
          rt_FREE(ptr);

27229
        }

27230


27231
        {

27232
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27233
          rt_FREE(ptr);

27234
        }

27235


27236
        rt_FREE(ssGetPortInfoForInputs(rts));

27237
        rt_FREE(ssGetPortInfoForOutputs(rts));

27238
      }

27239
    }

27240


27241
    /* RTW Generated Level2 S-Function Block: '<S169>/Generated S-Function' (Gain_sf) */

27242
    {

27243
      SimStruct *rts = ssGetSFunction(S, 85);

27244
      if (rts != (NULL) ) {

27245
        rt_FREE(ssGetSampleTimePtr(rts));

27246
        rt_FREE(ssGetOffsetTimePtr(rts));

27247
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27248


27249
        /* Destroy model methods 3 */

27250
        {

27251
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27252
          rt_FREE(methods3);

27253
        }

27254


27255
        /* Destroy model methods 2 */

27256
        {

27257
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27258
          rt_FREE(methods2);

27259
        }

27260


27261
        /* Destroy state auxilliary information */

27262
        {

27263
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27264
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27265
          rt_FREE(periodicStatesInfo);

27266
          rt_FREE(statesInfo2);

27267
        }

27268


27269
        {

27270
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27271
          rt_FREE(ptr);

27272
        }

27273


27274
        {

27275
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27276
          rt_FREE(ptr);

27277
        }

27278


27279
        rt_FREE(ssGetPortInfoForInputs(rts));

27280
        rt_FREE(ssGetPortInfoForOutputs(rts));

27281
      }

27282
    }

27283


27284
    /* RTW Generated Level2 S-Function Block: '<S160>/Generated S-Function3' (IntegratorLimited_sf) */

27285
    {

27286
      SimStruct *rts = ssGetSFunction(S, 86);

27287
      if (rts != (NULL) ) {

27288
        rt_FREE(ssGetSampleTimePtr(rts));

27289
        rt_FREE(ssGetOffsetTimePtr(rts));

27290
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27291


27292
        /* Destroy model methods 3 */

27293
        {

27294
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27295
          rt_FREE(methods3);

27296
        }

27297


27298
        /* Destroy model methods 2 */

27299
        {

27300
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27301
          rt_FREE(methods2);

27302
        }

27303


27304
        /* Destroy state auxilliary information */

27305
        {

27306
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27307
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27308
          rt_FREE(periodicStatesInfo);

27309
          rt_FREE(statesInfo2);

27310
        }

27311


27312
        {

27313
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27314
          rt_FREE(ptr);

27315
        }

27316


27317
        {

27318
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27319
          rt_FREE(ptr);

27320
        }

27321


27322
        {

27323
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27324
          rt_FREE(ptr);

27325
        }

27326


27327
        {

27328
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

27329
          rt_FREE(ptr);

27330
        }

27331


27332
        {

27333
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

27334
          rt_FREE(ptr);

27335
        }

27336


27337
        {

27338
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

27339
          rt_FREE(ptr);

27340
        }

27341


27342
        {

27343
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

27344
          rt_FREE(ptr);

27345
        }

27346


27347
        rt_FREE(ssGetPortInfoForInputs(rts));

27348
        rt_FREE(ssGetPortInfoForOutputs(rts));

27349
        rt_FREE(ssGetSFcnDWork(rts));

27350
      }

27351
    }

27352


27353
    /* RTW Generated Level2 S-Function Block: '<S129>/Generated S-Function6' (Res180_sf) */

27354
    {

27355
      SimStruct *rts = ssGetSFunction(S, 87);

27356
      if (rts != (NULL) ) {

27357
        rt_FREE(ssGetSampleTimePtr(rts));

27358
        rt_FREE(ssGetOffsetTimePtr(rts));

27359
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27360


27361
        /* Destroy model methods 3 */

27362
        {

27363
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27364
          rt_FREE(methods3);

27365
        }

27366


27367
        /* Destroy model methods 2 */

27368
        {

27369
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27370
          rt_FREE(methods2);

27371
        }

27372


27373
        /* Destroy state auxilliary information */

27374
        {

27375
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27376
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27377
          rt_FREE(periodicStatesInfo);

27378
          rt_FREE(statesInfo2);

27379
        }

27380


27381
        {

27382
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27383
          rt_FREE(ptr);

27384
        }

27385


27386
        rt_FREE(ssGetPortInfoForInputs(rts));

27387
        rt_FREE(ssGetPortInfoForOutputs(rts));

27388
      }

27389
    }

27390


27391
    /* RTW Generated Level2 S-Function Block: '<S162>/Generated S-Function1' (SaturationLimiter_sf) */

27392
    {

27393
      SimStruct *rts = ssGetSFunction(S, 88);

27394
      if (rts != (NULL) ) {

27395
        rt_FREE(ssGetSampleTimePtr(rts));

27396
        rt_FREE(ssGetOffsetTimePtr(rts));

27397
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27398


27399
        /* Destroy model methods 3 */

27400
        {

27401
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27402
          rt_FREE(methods3);

27403
        }

27404


27405
        /* Destroy model methods 2 */

27406
        {

27407
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27408
          rt_FREE(methods2);

27409
        }

27410


27411
        /* Destroy state auxilliary information */

27412
        {

27413
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27414
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27415
          rt_FREE(periodicStatesInfo);

27416
          rt_FREE(statesInfo2);

27417
        }

27418


27419
        {

27420
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27421
          rt_FREE(ptr);

27422
        }

27423


27424
        {

27425
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27426
          rt_FREE(ptr);

27427
        }

27428


27429
        {

27430
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27431
          rt_FREE(ptr);

27432
        }

27433


27434
        rt_FREE(ssGetPortInfoForInputs(rts));

27435
        rt_FREE(ssGetPortInfoForOutputs(rts));

27436
      }

27437
    }

27438


27439
    /* RTW Generated Level2 S-Function Block: '<S167>/Generated S-Function' (Gain_sf) */

27440
    {

27441
      SimStruct *rts = ssGetSFunction(S, 89);

27442
      if (rts != (NULL) ) {

27443
        rt_FREE(ssGetSampleTimePtr(rts));

27444
        rt_FREE(ssGetOffsetTimePtr(rts));

27445
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27446


27447
        /* Destroy model methods 3 */

27448
        {

27449
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27450
          rt_FREE(methods3);

27451
        }

27452


27453
        /* Destroy model methods 2 */

27454
        {

27455
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27456
          rt_FREE(methods2);

27457
        }

27458


27459
        /* Destroy state auxilliary information */

27460
        {

27461
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27462
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27463
          rt_FREE(periodicStatesInfo);

27464
          rt_FREE(statesInfo2);

27465
        }

27466


27467
        {

27468
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27469
          rt_FREE(ptr);

27470
        }

27471


27472
        {

27473
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27474
          rt_FREE(ptr);

27475
        }

27476


27477
        rt_FREE(ssGetPortInfoForInputs(rts));

27478
        rt_FREE(ssGetPortInfoForOutputs(rts));

27479
      }

27480
    }

27481


27482
    /* RTW Generated Level2 S-Function Block: '<S161>/Generated S-Function1' (SaturationLimiter_sf) */

27483
    {

27484
      SimStruct *rts = ssGetSFunction(S, 90);

27485
      if (rts != (NULL) ) {

27486
        rt_FREE(ssGetSampleTimePtr(rts));

27487
        rt_FREE(ssGetOffsetTimePtr(rts));

27488
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27489


27490
        /* Destroy model methods 3 */

27491
        {

27492
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27493
          rt_FREE(methods3);

27494
        }

27495


27496
        /* Destroy model methods 2 */

27497
        {

27498
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27499
          rt_FREE(methods2);

27500
        }

27501


27502
        /* Destroy state auxilliary information */

27503
        {

27504
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27505
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27506
          rt_FREE(periodicStatesInfo);

27507
          rt_FREE(statesInfo2);

27508
        }

27509


27510
        {

27511
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27512
          rt_FREE(ptr);

27513
        }

27514


27515
        {

27516
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27517
          rt_FREE(ptr);

27518
        }

27519


27520
        {

27521
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27522
          rt_FREE(ptr);

27523
        }

27524


27525
        rt_FREE(ssGetPortInfoForInputs(rts));

27526
        rt_FREE(ssGetPortInfoForOutputs(rts));

27527
      }

27528
    }

27529


27530
    /* RTW Generated Level2 S-Function Block: '<S129>/Generated S-Function1' (Res180_sf) */

27531
    {

27532
      SimStruct *rts = ssGetSFunction(S, 91);

27533
      if (rts != (NULL) ) {

27534
        rt_FREE(ssGetSampleTimePtr(rts));

27535
        rt_FREE(ssGetOffsetTimePtr(rts));

27536
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27537


27538
        /* Destroy model methods 3 */

27539
        {

27540
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27541
          rt_FREE(methods3);

27542
        }

27543


27544
        /* Destroy model methods 2 */

27545
        {

27546
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27547
          rt_FREE(methods2);

27548
        }

27549


27550
        /* Destroy state auxilliary information */

27551
        {

27552
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27553
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27554
          rt_FREE(periodicStatesInfo);

27555
          rt_FREE(statesInfo2);

27556
        }

27557


27558
        {

27559
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27560
          rt_FREE(ptr);

27561
        }

27562


27563
        rt_FREE(ssGetPortInfoForInputs(rts));

27564
        rt_FREE(ssGetPortInfoForOutputs(rts));

27565
      }

27566
    }

27567


27568
    /* RTW Generated Level2 S-Function Block: '<S166>/Generated S-Function' (Gain_sf) */

27569
    {

27570
      SimStruct *rts = ssGetSFunction(S, 92);

27571
      if (rts != (NULL) ) {

27572
        rt_FREE(ssGetSampleTimePtr(rts));

27573
        rt_FREE(ssGetOffsetTimePtr(rts));

27574
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27575


27576
        /* Destroy model methods 3 */

27577
        {

27578
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27579
          rt_FREE(methods3);

27580
        }

27581


27582
        /* Destroy model methods 2 */

27583
        {

27584
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27585
          rt_FREE(methods2);

27586
        }

27587


27588
        /* Destroy state auxilliary information */

27589
        {

27590
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27591
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27592
          rt_FREE(periodicStatesInfo);

27593
          rt_FREE(statesInfo2);

27594
        }

27595


27596
        {

27597
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27598
          rt_FREE(ptr);

27599
        }

27600


27601
        {

27602
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27603
          rt_FREE(ptr);

27604
        }

27605


27606
        rt_FREE(ssGetPortInfoForInputs(rts));

27607
        rt_FREE(ssGetPortInfoForOutputs(rts));

27608
      }

27609
    }

27610


27611
    /* RTW Generated Level2 S-Function Block: '<S159>/Generated S-Function4' (LagFilter_sf) */

27612
    {

27613
      SimStruct *rts = ssGetSFunction(S, 93);

27614
      if (rts != (NULL) ) {

27615
        rt_FREE(ssGetSampleTimePtr(rts));

27616
        rt_FREE(ssGetOffsetTimePtr(rts));

27617
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27618


27619
        /* Destroy model methods 3 */

27620
        {

27621
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27622
          rt_FREE(methods3);

27623
        }

27624


27625
        /* Destroy model methods 2 */

27626
        {

27627
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27628
          rt_FREE(methods2);

27629
        }

27630


27631
        /* Destroy state auxilliary information */

27632
        {

27633
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27634
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27635
          rt_FREE(periodicStatesInfo);

27636
          rt_FREE(statesInfo2);

27637
        }

27638


27639
        {

27640
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27641
          rt_FREE(ptr);

27642
        }

27643


27644
        {

27645
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27646
          rt_FREE(ptr);

27647
        }

27648


27649
        {

27650
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27651
          rt_FREE(ptr);

27652
        }

27653


27654
        {

27655
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

27656
          rt_FREE(ptr);

27657
        }

27658


27659
        {

27660
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

27661
          rt_FREE(ptr);

27662
        }

27663


27664
        {

27665
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

27666
          rt_FREE(ptr);

27667
        }

27668


27669
        rt_FREE(ssGetPortInfoForInputs(rts));

27670
        rt_FREE(ssGetPortInfoForOutputs(rts));

27671
        rt_FREE(ssGetSFcnDWork(rts));

27672
      }

27673
    }

27674


27675
    /* RTW Generated Level2 S-Function Block: '<S123>/Generated S-Function5' (Fader_sf) */

27676
    {

27677
      SimStruct *rts = ssGetSFunction(S, 94);

27678
      if (rts != (NULL) ) {

27679
        rt_FREE(ssGetSampleTimePtr(rts));

27680
        rt_FREE(ssGetOffsetTimePtr(rts));

27681
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27682


27683
        /* Destroy model methods 3 */

27684
        {

27685
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27686
          rt_FREE(methods3);

27687
        }

27688


27689
        /* Destroy model methods 2 */

27690
        {

27691
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27692
          rt_FREE(methods2);

27693
        }

27694


27695
        /* Destroy state auxilliary information */

27696
        {

27697
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27698
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27699
          rt_FREE(periodicStatesInfo);

27700
          rt_FREE(statesInfo2);

27701
        }

27702


27703
        {

27704
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27705
          rt_FREE(ptr);

27706
        }

27707


27708
        {

27709
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27710
          rt_FREE(ptr);

27711
        }

27712


27713
        {

27714
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27715
          rt_FREE(ptr);

27716
        }

27717


27718
        {

27719
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

27720
          rt_FREE(ptr);

27721
        }

27722


27723
        {

27724
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

27725
          rt_FREE(ptr);

27726
        }

27727


27728
        {

27729
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

27730
          rt_FREE(ptr);

27731
        }

27732


27733
        rt_FREE(ssGetPortInfoForInputs(rts));

27734
        rt_FREE(ssGetPortInfoForOutputs(rts));

27735
        rt_FREE(ssGetSFcnDWork(rts));

27736
      }

27737
    }

27738


27739
    /* RTW Generated Level2 S-Function Block: '<S163>/Generated S-Function1' (SaturationLimiter_sf) */

27740
    {

27741
      SimStruct *rts = ssGetSFunction(S, 95);

27742
      if (rts != (NULL) ) {

27743
        rt_FREE(ssGetSampleTimePtr(rts));

27744
        rt_FREE(ssGetOffsetTimePtr(rts));

27745
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27746


27747
        /* Destroy model methods 3 */

27748
        {

27749
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27750
          rt_FREE(methods3);

27751
        }

27752


27753
        /* Destroy model methods 2 */

27754
        {

27755
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27756
          rt_FREE(methods2);

27757
        }

27758


27759
        /* Destroy state auxilliary information */

27760
        {

27761
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27762
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27763
          rt_FREE(periodicStatesInfo);

27764
          rt_FREE(statesInfo2);

27765
        }

27766


27767
        {

27768
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27769
          rt_FREE(ptr);

27770
        }

27771


27772
        {

27773
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27774
          rt_FREE(ptr);

27775
        }

27776


27777
        {

27778
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27779
          rt_FREE(ptr);

27780
        }

27781


27782
        rt_FREE(ssGetPortInfoForInputs(rts));

27783
        rt_FREE(ssGetPortInfoForOutputs(rts));

27784
      }

27785
    }

27786


27787
    /* RTW Generated Level2 S-Function Block: '<S165>/Generated S-Function1' (SaturationLimiter_sf) */

27788
    {

27789
      SimStruct *rts = ssGetSFunction(S, 96);

27790
      if (rts != (NULL) ) {

27791
        rt_FREE(ssGetSampleTimePtr(rts));

27792
        rt_FREE(ssGetOffsetTimePtr(rts));

27793
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27794


27795
        /* Destroy model methods 3 */

27796
        {

27797
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27798
          rt_FREE(methods3);

27799
        }

27800


27801
        /* Destroy model methods 2 */

27802
        {

27803
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27804
          rt_FREE(methods2);

27805
        }

27806


27807
        /* Destroy state auxilliary information */

27808
        {

27809
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27810
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27811
          rt_FREE(periodicStatesInfo);

27812
          rt_FREE(statesInfo2);

27813
        }

27814


27815
        {

27816
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27817
          rt_FREE(ptr);

27818
        }

27819


27820
        {

27821
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27822
          rt_FREE(ptr);

27823
        }

27824


27825
        {

27826
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27827
          rt_FREE(ptr);

27828
        }

27829


27830
        rt_FREE(ssGetPortInfoForInputs(rts));

27831
        rt_FREE(ssGetPortInfoForOutputs(rts));

27832
      }

27833
    }

27834


27835
    /* RTW Generated Level2 S-Function Block: '<S190>/Generated S-Function4' (LagFilter_sf) */

27836
    {

27837
      SimStruct *rts = ssGetSFunction(S, 97);

27838
      if (rts != (NULL) ) {

27839
        rt_FREE(ssGetSampleTimePtr(rts));

27840
        rt_FREE(ssGetOffsetTimePtr(rts));

27841
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27842


27843
        /* Destroy model methods 3 */

27844
        {

27845
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27846
          rt_FREE(methods3);

27847
        }

27848


27849
        /* Destroy model methods 2 */

27850
        {

27851
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27852
          rt_FREE(methods2);

27853
        }

27854


27855
        /* Destroy state auxilliary information */

27856
        {

27857
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27858
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27859
          rt_FREE(periodicStatesInfo);

27860
          rt_FREE(statesInfo2);

27861
        }

27862


27863
        {

27864
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27865
          rt_FREE(ptr);

27866
        }

27867


27868
        {

27869
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27870
          rt_FREE(ptr);

27871
        }

27872


27873
        {

27874
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27875
          rt_FREE(ptr);

27876
        }

27877


27878
        {

27879
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

27880
          rt_FREE(ptr);

27881
        }

27882


27883
        {

27884
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

27885
          rt_FREE(ptr);

27886
        }

27887


27888
        {

27889
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

27890
          rt_FREE(ptr);

27891
        }

27892


27893
        rt_FREE(ssGetPortInfoForInputs(rts));

27894
        rt_FREE(ssGetPortInfoForOutputs(rts));

27895
        rt_FREE(ssGetSFcnDWork(rts));

27896
      }

27897
    }

27898


27899
    /* RTW Generated Level2 S-Function Block: '<S191>/Generated S-Function4' (LagFilter_sf) */

27900
    {

27901
      SimStruct *rts = ssGetSFunction(S, 98);

27902
      if (rts != (NULL) ) {

27903
        rt_FREE(ssGetSampleTimePtr(rts));

27904
        rt_FREE(ssGetOffsetTimePtr(rts));

27905
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27906


27907
        /* Destroy model methods 3 */

27908
        {

27909
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27910
          rt_FREE(methods3);

27911
        }

27912


27913
        /* Destroy model methods 2 */

27914
        {

27915
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27916
          rt_FREE(methods2);

27917
        }

27918


27919
        /* Destroy state auxilliary information */

27920
        {

27921
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27922
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27923
          rt_FREE(periodicStatesInfo);

27924
          rt_FREE(statesInfo2);

27925
        }

27926


27927
        {

27928
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27929
          rt_FREE(ptr);

27930
        }

27931


27932
        {

27933
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

27934
          rt_FREE(ptr);

27935
        }

27936


27937
        {

27938
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

27939
          rt_FREE(ptr);

27940
        }

27941


27942
        {

27943
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

27944
          rt_FREE(ptr);

27945
        }

27946


27947
        {

27948
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

27949
          rt_FREE(ptr);

27950
        }

27951


27952
        {

27953
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

27954
          rt_FREE(ptr);

27955
        }

27956


27957
        rt_FREE(ssGetPortInfoForInputs(rts));

27958
        rt_FREE(ssGetPortInfoForOutputs(rts));

27959
        rt_FREE(ssGetSFcnDWork(rts));

27960
      }

27961
    }

27962


27963
    /* RTW Generated Level2 S-Function Block: '<S193>/Generated S-Function' (Res180_sf) */

27964
    {

27965
      SimStruct *rts = ssGetSFunction(S, 99);

27966
      if (rts != (NULL) ) {

27967
        rt_FREE(ssGetSampleTimePtr(rts));

27968
        rt_FREE(ssGetOffsetTimePtr(rts));

27969
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

27970


27971
        /* Destroy model methods 3 */

27972
        {

27973
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

27974
          rt_FREE(methods3);

27975
        }

27976


27977
        /* Destroy model methods 2 */

27978
        {

27979
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

27980
          rt_FREE(methods2);

27981
        }

27982


27983
        /* Destroy state auxilliary information */

27984
        {

27985
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

27986
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

27987
          rt_FREE(periodicStatesInfo);

27988
          rt_FREE(statesInfo2);

27989
        }

27990


27991
        {

27992
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

27993
          rt_FREE(ptr);

27994
        }

27995


27996
        rt_FREE(ssGetPortInfoForInputs(rts));

27997
        rt_FREE(ssGetPortInfoForOutputs(rts));

27998
      }

27999
    }

28000


28001
    /* RTW Generated Level2 S-Function Block: '<S198>/Generated S-Function1' (SaturationLimiter_sf) */

28002
    {

28003
      SimStruct *rts = ssGetSFunction(S, 100);

28004
      if (rts != (NULL) ) {

28005
        rt_FREE(ssGetSampleTimePtr(rts));

28006
        rt_FREE(ssGetOffsetTimePtr(rts));

28007
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28008


28009
        /* Destroy model methods 3 */

28010
        {

28011
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28012
          rt_FREE(methods3);

28013
        }

28014


28015
        /* Destroy model methods 2 */

28016
        {

28017
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28018
          rt_FREE(methods2);

28019
        }

28020


28021
        /* Destroy state auxilliary information */

28022
        {

28023
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28024
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28025
          rt_FREE(periodicStatesInfo);

28026
          rt_FREE(statesInfo2);

28027
        }

28028


28029
        {

28030
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28031
          rt_FREE(ptr);

28032
        }

28033


28034
        {

28035
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28036
          rt_FREE(ptr);

28037
        }

28038


28039
        {

28040
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28041
          rt_FREE(ptr);

28042
        }

28043


28044
        rt_FREE(ssGetPortInfoForInputs(rts));

28045
        rt_FREE(ssGetPortInfoForOutputs(rts));

28046
      }

28047
    }

28048


28049
    /* RTW Generated Level2 S-Function Block: '<S199>/Generated S-Function' (Gain_sf) */

28050
    {

28051
      SimStruct *rts = ssGetSFunction(S, 101);

28052
      if (rts != (NULL) ) {

28053
        rt_FREE(ssGetSampleTimePtr(rts));

28054
        rt_FREE(ssGetOffsetTimePtr(rts));

28055
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28056


28057
        /* Destroy model methods 3 */

28058
        {

28059
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28060
          rt_FREE(methods3);

28061
        }

28062


28063
        /* Destroy model methods 2 */

28064
        {

28065
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28066
          rt_FREE(methods2);

28067
        }

28068


28069
        /* Destroy state auxilliary information */

28070
        {

28071
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28072
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28073
          rt_FREE(periodicStatesInfo);

28074
          rt_FREE(statesInfo2);

28075
        }

28076


28077
        {

28078
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28079
          rt_FREE(ptr);

28080
        }

28081


28082
        {

28083
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28084
          rt_FREE(ptr);

28085
        }

28086


28087
        rt_FREE(ssGetPortInfoForInputs(rts));

28088
        rt_FREE(ssGetPortInfoForOutputs(rts));

28089
      }

28090
    }

28091


28092
    /* RTW Generated Level2 S-Function Block: '<S197>/Generated S-Function2' (LeadLagFilter_sf) */

28093
    {

28094
      SimStruct *rts = ssGetSFunction(S, 102);

28095
      if (rts != (NULL) ) {

28096
        rt_FREE(ssGetSampleTimePtr(rts));

28097
        rt_FREE(ssGetOffsetTimePtr(rts));

28098
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28099


28100
        /* Destroy model methods 3 */

28101
        {

28102
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28103
          rt_FREE(methods3);

28104
        }

28105


28106
        /* Destroy model methods 2 */

28107
        {

28108
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28109
          rt_FREE(methods2);

28110
        }

28111


28112
        /* Destroy state auxilliary information */

28113
        {

28114
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28115
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28116
          rt_FREE(periodicStatesInfo);

28117
          rt_FREE(statesInfo2);

28118
        }

28119


28120
        {

28121
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28122
          rt_FREE(ptr);

28123
        }

28124


28125
        {

28126
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28127
          rt_FREE(ptr);

28128
        }

28129


28130
        {

28131
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28132
          rt_FREE(ptr);

28133
        }

28134


28135
        {

28136
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

28137
          rt_FREE(ptr);

28138
        }

28139


28140
        {

28141
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

28142
          rt_FREE(ptr);

28143
        }

28144


28145
        {

28146
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

28147
          rt_FREE(ptr);

28148
        }

28149


28150
        {

28151
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

28152
          rt_FREE(ptr);

28153
        }

28154


28155
        rt_FREE(ssGetPortInfoForInputs(rts));

28156
        rt_FREE(ssGetPortInfoForOutputs(rts));

28157
        rt_FREE(ssGetSFcnDWork(rts));

28158
      }

28159
    }

28160


28161
    /* RTW Generated Level2 S-Function Block: '<S196>/Generated S-Function4' (LagFilter_sf) */

28162
    {

28163
      SimStruct *rts = ssGetSFunction(S, 103);

28164
      if (rts != (NULL) ) {

28165
        rt_FREE(ssGetSampleTimePtr(rts));

28166
        rt_FREE(ssGetOffsetTimePtr(rts));

28167
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28168


28169
        /* Destroy model methods 3 */

28170
        {

28171
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28172
          rt_FREE(methods3);

28173
        }

28174


28175
        /* Destroy model methods 2 */

28176
        {

28177
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28178
          rt_FREE(methods2);

28179
        }

28180


28181
        /* Destroy state auxilliary information */

28182
        {

28183
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28184
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28185
          rt_FREE(periodicStatesInfo);

28186
          rt_FREE(statesInfo2);

28187
        }

28188


28189
        {

28190
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28191
          rt_FREE(ptr);

28192
        }

28193


28194
        {

28195
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28196
          rt_FREE(ptr);

28197
        }

28198


28199
        {

28200
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28201
          rt_FREE(ptr);

28202
        }

28203


28204
        {

28205
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

28206
          rt_FREE(ptr);

28207
        }

28208


28209
        {

28210
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

28211
          rt_FREE(ptr);

28212
        }

28213


28214
        {

28215
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

28216
          rt_FREE(ptr);

28217
        }

28218


28219
        rt_FREE(ssGetPortInfoForInputs(rts));

28220
        rt_FREE(ssGetPortInfoForOutputs(rts));

28221
        rt_FREE(ssGetSFcnDWork(rts));

28222
      }

28223
    }

28224


28225
    /* RTW Generated Level2 S-Function Block: '<S204>/Generated S-Function' (Gain_sf) */

28226
    {

28227
      SimStruct *rts = ssGetSFunction(S, 104);

28228
      if (rts != (NULL) ) {

28229
        rt_FREE(ssGetSampleTimePtr(rts));

28230
        rt_FREE(ssGetOffsetTimePtr(rts));

28231
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28232


28233
        /* Destroy model methods 3 */

28234
        {

28235
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28236
          rt_FREE(methods3);

28237
        }

28238


28239
        /* Destroy model methods 2 */

28240
        {

28241
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28242
          rt_FREE(methods2);

28243
        }

28244


28245
        /* Destroy state auxilliary information */

28246
        {

28247
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28248
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28249
          rt_FREE(periodicStatesInfo);

28250
          rt_FREE(statesInfo2);

28251
        }

28252


28253
        {

28254
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28255
          rt_FREE(ptr);

28256
        }

28257


28258
        {

28259
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28260
          rt_FREE(ptr);

28261
        }

28262


28263
        rt_FREE(ssGetPortInfoForInputs(rts));

28264
        rt_FREE(ssGetPortInfoForOutputs(rts));

28265
      }

28266
    }

28267


28268
    /* RTW Generated Level2 S-Function Block: '<S203>/Generated S-Function1' (SaturationLimiter_sf) */

28269
    {

28270
      SimStruct *rts = ssGetSFunction(S, 105);

28271
      if (rts != (NULL) ) {

28272
        rt_FREE(ssGetSampleTimePtr(rts));

28273
        rt_FREE(ssGetOffsetTimePtr(rts));

28274
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28275


28276
        /* Destroy model methods 3 */

28277
        {

28278
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28279
          rt_FREE(methods3);

28280
        }

28281


28282
        /* Destroy model methods 2 */

28283
        {

28284
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28285
          rt_FREE(methods2);

28286
        }

28287


28288
        /* Destroy state auxilliary information */

28289
        {

28290
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28291
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28292
          rt_FREE(periodicStatesInfo);

28293
          rt_FREE(statesInfo2);

28294
        }

28295


28296
        {

28297
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28298
          rt_FREE(ptr);

28299
        }

28300


28301
        {

28302
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28303
          rt_FREE(ptr);

28304
        }

28305


28306
        {

28307
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28308
          rt_FREE(ptr);

28309
        }

28310


28311
        rt_FREE(ssGetPortInfoForInputs(rts));

28312
        rt_FREE(ssGetPortInfoForOutputs(rts));

28313
      }

28314
    }

28315


28316
    /* RTW Generated Level2 S-Function Block: '<S206>/Generated S-Function' (Gain_sf) */

28317
    {

28318
      SimStruct *rts = ssGetSFunction(S, 106);

28319
      if (rts != (NULL) ) {

28320
        rt_FREE(ssGetSampleTimePtr(rts));

28321
        rt_FREE(ssGetOffsetTimePtr(rts));

28322
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28323


28324
        /* Destroy model methods 3 */

28325
        {

28326
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28327
          rt_FREE(methods3);

28328
        }

28329


28330
        /* Destroy model methods 2 */

28331
        {

28332
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28333
          rt_FREE(methods2);

28334
        }

28335


28336
        /* Destroy state auxilliary information */

28337
        {

28338
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28339
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28340
          rt_FREE(periodicStatesInfo);

28341
          rt_FREE(statesInfo2);

28342
        }

28343


28344
        {

28345
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28346
          rt_FREE(ptr);

28347
        }

28348


28349
        {

28350
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28351
          rt_FREE(ptr);

28352
        }

28353


28354
        rt_FREE(ssGetPortInfoForInputs(rts));

28355
        rt_FREE(ssGetPortInfoForOutputs(rts));

28356
      }

28357
    }

28358


28359
    /* RTW Generated Level2 S-Function Block: '<S194>/Generated S-Function6' (Res180_sf) */

28360
    {

28361
      SimStruct *rts = ssGetSFunction(S, 107);

28362
      if (rts != (NULL) ) {

28363
        rt_FREE(ssGetSampleTimePtr(rts));

28364
        rt_FREE(ssGetOffsetTimePtr(rts));

28365
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28366


28367
        /* Destroy model methods 3 */

28368
        {

28369
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28370
          rt_FREE(methods3);

28371
        }

28372


28373
        /* Destroy model methods 2 */

28374
        {

28375
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28376
          rt_FREE(methods2);

28377
        }

28378


28379
        /* Destroy state auxilliary information */

28380
        {

28381
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28382
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28383
          rt_FREE(periodicStatesInfo);

28384
          rt_FREE(statesInfo2);

28385
        }

28386


28387
        {

28388
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28389
          rt_FREE(ptr);

28390
        }

28391


28392
        rt_FREE(ssGetPortInfoForInputs(rts));

28393
        rt_FREE(ssGetPortInfoForOutputs(rts));

28394
      }

28395
    }

28396


28397
    /* RTW Generated Level2 S-Function Block: '<S202>/Generated S-Function1' (SaturationLimiter_sf) */

28398
    {

28399
      SimStruct *rts = ssGetSFunction(S, 108);

28400
      if (rts != (NULL) ) {

28401
        rt_FREE(ssGetSampleTimePtr(rts));

28402
        rt_FREE(ssGetOffsetTimePtr(rts));

28403
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28404


28405
        /* Destroy model methods 3 */

28406
        {

28407
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28408
          rt_FREE(methods3);

28409
        }

28410


28411
        /* Destroy model methods 2 */

28412
        {

28413
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28414
          rt_FREE(methods2);

28415
        }

28416


28417
        /* Destroy state auxilliary information */

28418
        {

28419
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28420
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28421
          rt_FREE(periodicStatesInfo);

28422
          rt_FREE(statesInfo2);

28423
        }

28424


28425
        {

28426
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28427
          rt_FREE(ptr);

28428
        }

28429


28430
        {

28431
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28432
          rt_FREE(ptr);

28433
        }

28434


28435
        {

28436
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28437
          rt_FREE(ptr);

28438
        }

28439


28440
        rt_FREE(ssGetPortInfoForInputs(rts));

28441
        rt_FREE(ssGetPortInfoForOutputs(rts));

28442
      }

28443
    }

28444


28445
    /* RTW Generated Level2 S-Function Block: '<S205>/Generated S-Function' (Gain_sf) */

28446
    {

28447
      SimStruct *rts = ssGetSFunction(S, 109);

28448
      if (rts != (NULL) ) {

28449
        rt_FREE(ssGetSampleTimePtr(rts));

28450
        rt_FREE(ssGetOffsetTimePtr(rts));

28451
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28452


28453
        /* Destroy model methods 3 */

28454
        {

28455
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28456
          rt_FREE(methods3);

28457
        }

28458


28459
        /* Destroy model methods 2 */

28460
        {

28461
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28462
          rt_FREE(methods2);

28463
        }

28464


28465
        /* Destroy state auxilliary information */

28466
        {

28467
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28468
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28469
          rt_FREE(periodicStatesInfo);

28470
          rt_FREE(statesInfo2);

28471
        }

28472


28473
        {

28474
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28475
          rt_FREE(ptr);

28476
        }

28477


28478
        {

28479
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28480
          rt_FREE(ptr);

28481
        }

28482


28483
        rt_FREE(ssGetPortInfoForInputs(rts));

28484
        rt_FREE(ssGetPortInfoForOutputs(rts));

28485
      }

28486
    }

28487


28488
    /* RTW Generated Level2 S-Function Block: '<S201>/Generated S-Function2' (LeadLagFilter_sf) */

28489
    {

28490
      SimStruct *rts = ssGetSFunction(S, 110);

28491
      if (rts != (NULL) ) {

28492
        rt_FREE(ssGetSampleTimePtr(rts));

28493
        rt_FREE(ssGetOffsetTimePtr(rts));

28494
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28495


28496
        /* Destroy model methods 3 */

28497
        {

28498
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28499
          rt_FREE(methods3);

28500
        }

28501


28502
        /* Destroy model methods 2 */

28503
        {

28504
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28505
          rt_FREE(methods2);

28506
        }

28507


28508
        /* Destroy state auxilliary information */

28509
        {

28510
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28511
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28512
          rt_FREE(periodicStatesInfo);

28513
          rt_FREE(statesInfo2);

28514
        }

28515


28516
        {

28517
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28518
          rt_FREE(ptr);

28519
        }

28520


28521
        {

28522
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28523
          rt_FREE(ptr);

28524
        }

28525


28526
        {

28527
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28528
          rt_FREE(ptr);

28529
        }

28530


28531
        {

28532
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

28533
          rt_FREE(ptr);

28534
        }

28535


28536
        {

28537
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

28538
          rt_FREE(ptr);

28539
        }

28540


28541
        {

28542
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

28543
          rt_FREE(ptr);

28544
        }

28545


28546
        {

28547
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

28548
          rt_FREE(ptr);

28549
        }

28550


28551
        rt_FREE(ssGetPortInfoForInputs(rts));

28552
        rt_FREE(ssGetPortInfoForOutputs(rts));

28553
        rt_FREE(ssGetSFcnDWork(rts));

28554
      }

28555
    }

28556


28557
    /* RTW Generated Level2 S-Function Block: '<S200>/Generated S-Function4' (LagFilter_sf) */

28558
    {

28559
      SimStruct *rts = ssGetSFunction(S, 111);

28560
      if (rts != (NULL) ) {

28561
        rt_FREE(ssGetSampleTimePtr(rts));

28562
        rt_FREE(ssGetOffsetTimePtr(rts));

28563
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28564


28565
        /* Destroy model methods 3 */

28566
        {

28567
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28568
          rt_FREE(methods3);

28569
        }

28570


28571
        /* Destroy model methods 2 */

28572
        {

28573
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28574
          rt_FREE(methods2);

28575
        }

28576


28577
        /* Destroy state auxilliary information */

28578
        {

28579
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28580
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28581
          rt_FREE(periodicStatesInfo);

28582
          rt_FREE(statesInfo2);

28583
        }

28584


28585
        {

28586
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28587
          rt_FREE(ptr);

28588
        }

28589


28590
        {

28591
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28592
          rt_FREE(ptr);

28593
        }

28594


28595
        {

28596
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28597
          rt_FREE(ptr);

28598
        }

28599


28600
        {

28601
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

28602
          rt_FREE(ptr);

28603
        }

28604


28605
        {

28606
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

28607
          rt_FREE(ptr);

28608
        }

28609


28610
        {

28611
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

28612
          rt_FREE(ptr);

28613
        }

28614


28615
        rt_FREE(ssGetPortInfoForInputs(rts));

28616
        rt_FREE(ssGetPortInfoForOutputs(rts));

28617
        rt_FREE(ssGetSFcnDWork(rts));

28618
      }

28619
    }

28620


28621
    /* RTW Generated Level2 S-Function Block: '<S69>/Generated S-Function4' (LagFilter_sf) */

28622
    {

28623
      SimStruct *rts = ssGetSFunction(S, 112);

28624
      if (rts != (NULL) ) {

28625
        rt_FREE(ssGetSampleTimePtr(rts));

28626
        rt_FREE(ssGetOffsetTimePtr(rts));

28627
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28628


28629
        /* Destroy model methods 3 */

28630
        {

28631
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28632
          rt_FREE(methods3);

28633
        }

28634


28635
        /* Destroy model methods 2 */

28636
        {

28637
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28638
          rt_FREE(methods2);

28639
        }

28640


28641
        /* Destroy state auxilliary information */

28642
        {

28643
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28644
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28645
          rt_FREE(periodicStatesInfo);

28646
          rt_FREE(statesInfo2);

28647
        }

28648


28649
        {

28650
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28651
          rt_FREE(ptr);

28652
        }

28653


28654
        {

28655
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28656
          rt_FREE(ptr);

28657
        }

28658


28659
        {

28660
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28661
          rt_FREE(ptr);

28662
        }

28663


28664
        {

28665
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

28666
          rt_FREE(ptr);

28667
        }

28668


28669
        {

28670
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

28671
          rt_FREE(ptr);

28672
        }

28673


28674
        {

28675
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

28676
          rt_FREE(ptr);

28677
        }

28678


28679
        rt_FREE(ssGetPortInfoForInputs(rts));

28680
        rt_FREE(ssGetPortInfoForOutputs(rts));

28681
        rt_FREE(ssGetSFcnDWork(rts));

28682
      }

28683
    }

28684


28685
    /* RTW Generated Level2 S-Function Block: '<S70>/Generated S-Function4' (LagFilter_sf) */

28686
    {

28687
      SimStruct *rts = ssGetSFunction(S, 113);

28688
      if (rts != (NULL) ) {

28689
        rt_FREE(ssGetSampleTimePtr(rts));

28690
        rt_FREE(ssGetOffsetTimePtr(rts));

28691
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28692


28693
        /* Destroy model methods 3 */

28694
        {

28695
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28696
          rt_FREE(methods3);

28697
        }

28698


28699
        /* Destroy model methods 2 */

28700
        {

28701
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28702
          rt_FREE(methods2);

28703
        }

28704


28705
        /* Destroy state auxilliary information */

28706
        {

28707
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28708
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28709
          rt_FREE(periodicStatesInfo);

28710
          rt_FREE(statesInfo2);

28711
        }

28712


28713
        {

28714
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28715
          rt_FREE(ptr);

28716
        }

28717


28718
        {

28719
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28720
          rt_FREE(ptr);

28721
        }

28722


28723
        {

28724
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28725
          rt_FREE(ptr);

28726
        }

28727


28728
        {

28729
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

28730
          rt_FREE(ptr);

28731
        }

28732


28733
        {

28734
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

28735
          rt_FREE(ptr);

28736
        }

28737


28738
        {

28739
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

28740
          rt_FREE(ptr);

28741
        }

28742


28743
        rt_FREE(ssGetPortInfoForInputs(rts));

28744
        rt_FREE(ssGetPortInfoForOutputs(rts));

28745
        rt_FREE(ssGetSFcnDWork(rts));

28746
      }

28747
    }

28748


28749
    /* RTW Generated Level2 S-Function Block: '<S77>/Generated S-Function1' (SaturationLimiter_sf) */

28750
    {

28751
      SimStruct *rts = ssGetSFunction(S, 114);

28752
      if (rts != (NULL) ) {

28753
        rt_FREE(ssGetSampleTimePtr(rts));

28754
        rt_FREE(ssGetOffsetTimePtr(rts));

28755
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28756


28757
        /* Destroy model methods 3 */

28758
        {

28759
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28760
          rt_FREE(methods3);

28761
        }

28762


28763
        /* Destroy model methods 2 */

28764
        {

28765
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28766
          rt_FREE(methods2);

28767
        }

28768


28769
        /* Destroy state auxilliary information */

28770
        {

28771
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28772
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28773
          rt_FREE(periodicStatesInfo);

28774
          rt_FREE(statesInfo2);

28775
        }

28776


28777
        {

28778
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28779
          rt_FREE(ptr);

28780
        }

28781


28782
        {

28783
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28784
          rt_FREE(ptr);

28785
        }

28786


28787
        {

28788
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28789
          rt_FREE(ptr);

28790
        }

28791


28792
        rt_FREE(ssGetPortInfoForInputs(rts));

28793
        rt_FREE(ssGetPortInfoForOutputs(rts));

28794
      }

28795
    }

28796


28797
    /* RTW Generated Level2 S-Function Block: '<S79>/Generated S-Function' (Gain_sf) */

28798
    {

28799
      SimStruct *rts = ssGetSFunction(S, 115);

28800
      if (rts != (NULL) ) {

28801
        rt_FREE(ssGetSampleTimePtr(rts));

28802
        rt_FREE(ssGetOffsetTimePtr(rts));

28803
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28804


28805
        /* Destroy model methods 3 */

28806
        {

28807
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28808
          rt_FREE(methods3);

28809
        }

28810


28811
        /* Destroy model methods 2 */

28812
        {

28813
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28814
          rt_FREE(methods2);

28815
        }

28816


28817
        /* Destroy state auxilliary information */

28818
        {

28819
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28820
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28821
          rt_FREE(periodicStatesInfo);

28822
          rt_FREE(statesInfo2);

28823
        }

28824


28825
        {

28826
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28827
          rt_FREE(ptr);

28828
        }

28829


28830
        {

28831
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28832
          rt_FREE(ptr);

28833
        }

28834


28835
        rt_FREE(ssGetPortInfoForInputs(rts));

28836
        rt_FREE(ssGetPortInfoForOutputs(rts));

28837
      }

28838
    }

28839


28840
    /* RTW Generated Level2 S-Function Block: '<S72>/Generated S-Function6' (Res180_sf) */

28841
    {

28842
      SimStruct *rts = ssGetSFunction(S, 116);

28843
      if (rts != (NULL) ) {

28844
        rt_FREE(ssGetSampleTimePtr(rts));

28845
        rt_FREE(ssGetOffsetTimePtr(rts));

28846
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28847


28848
        /* Destroy model methods 3 */

28849
        {

28850
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28851
          rt_FREE(methods3);

28852
        }

28853


28854
        /* Destroy model methods 2 */

28855
        {

28856
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28857
          rt_FREE(methods2);

28858
        }

28859


28860
        /* Destroy state auxilliary information */

28861
        {

28862
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28863
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28864
          rt_FREE(periodicStatesInfo);

28865
          rt_FREE(statesInfo2);

28866
        }

28867


28868
        {

28869
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28870
          rt_FREE(ptr);

28871
        }

28872


28873
        rt_FREE(ssGetPortInfoForInputs(rts));

28874
        rt_FREE(ssGetPortInfoForOutputs(rts));

28875
      }

28876
    }

28877


28878
    /* RTW Generated Level2 S-Function Block: '<S76>/Generated S-Function1' (SaturationLimiter_sf) */

28879
    {

28880
      SimStruct *rts = ssGetSFunction(S, 117);

28881
      if (rts != (NULL) ) {

28882
        rt_FREE(ssGetSampleTimePtr(rts));

28883
        rt_FREE(ssGetOffsetTimePtr(rts));

28884
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28885


28886
        /* Destroy model methods 3 */

28887
        {

28888
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28889
          rt_FREE(methods3);

28890
        }

28891


28892
        /* Destroy model methods 2 */

28893
        {

28894
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28895
          rt_FREE(methods2);

28896
        }

28897


28898
        /* Destroy state auxilliary information */

28899
        {

28900
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28901
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28902
          rt_FREE(periodicStatesInfo);

28903
          rt_FREE(statesInfo2);

28904
        }

28905


28906
        {

28907
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28908
          rt_FREE(ptr);

28909
        }

28910


28911
        {

28912
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28913
          rt_FREE(ptr);

28914
        }

28915


28916
        {

28917
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

28918
          rt_FREE(ptr);

28919
        }

28920


28921
        rt_FREE(ssGetPortInfoForInputs(rts));

28922
        rt_FREE(ssGetPortInfoForOutputs(rts));

28923
      }

28924
    }

28925


28926
    /* RTW Generated Level2 S-Function Block: '<S78>/Generated S-Function' (Gain_sf) */

28927
    {

28928
      SimStruct *rts = ssGetSFunction(S, 118);

28929
      if (rts != (NULL) ) {

28930
        rt_FREE(ssGetSampleTimePtr(rts));

28931
        rt_FREE(ssGetOffsetTimePtr(rts));

28932
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28933


28934
        /* Destroy model methods 3 */

28935
        {

28936
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28937
          rt_FREE(methods3);

28938
        }

28939


28940
        /* Destroy model methods 2 */

28941
        {

28942
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28943
          rt_FREE(methods2);

28944
        }

28945


28946
        /* Destroy state auxilliary information */

28947
        {

28948
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28949
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28950
          rt_FREE(periodicStatesInfo);

28951
          rt_FREE(statesInfo2);

28952
        }

28953


28954
        {

28955
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28956
          rt_FREE(ptr);

28957
        }

28958


28959
        {

28960
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

28961
          rt_FREE(ptr);

28962
        }

28963


28964
        rt_FREE(ssGetPortInfoForInputs(rts));

28965
        rt_FREE(ssGetPortInfoForOutputs(rts));

28966
      }

28967
    }

28968


28969
    /* RTW Generated Level2 S-Function Block: '<S75>/Generated S-Function2' (LeadLagFilter_sf) */

28970
    {

28971
      SimStruct *rts = ssGetSFunction(S, 119);

28972
      if (rts != (NULL) ) {

28973
        rt_FREE(ssGetSampleTimePtr(rts));

28974
        rt_FREE(ssGetOffsetTimePtr(rts));

28975
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

28976


28977
        /* Destroy model methods 3 */

28978
        {

28979
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

28980
          rt_FREE(methods3);

28981
        }

28982


28983
        /* Destroy model methods 2 */

28984
        {

28985
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

28986
          rt_FREE(methods2);

28987
        }

28988


28989
        /* Destroy state auxilliary information */

28990
        {

28991
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

28992
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

28993
          rt_FREE(periodicStatesInfo);

28994
          rt_FREE(statesInfo2);

28995
        }

28996


28997
        {

28998
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

28999
          rt_FREE(ptr);

29000
        }

29001


29002
        {

29003
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29004
          rt_FREE(ptr);

29005
        }

29006


29007
        {

29008
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29009
          rt_FREE(ptr);

29010
        }

29011


29012
        {

29013
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29014
          rt_FREE(ptr);

29015
        }

29016


29017
        {

29018
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29019
          rt_FREE(ptr);

29020
        }

29021


29022
        {

29023
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

29024
          rt_FREE(ptr);

29025
        }

29026


29027
        {

29028
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

29029
          rt_FREE(ptr);

29030
        }

29031


29032
        rt_FREE(ssGetPortInfoForInputs(rts));

29033
        rt_FREE(ssGetPortInfoForOutputs(rts));

29034
        rt_FREE(ssGetSFcnDWork(rts));

29035
      }

29036
    }

29037


29038
    /* RTW Generated Level2 S-Function Block: '<S74>/Generated S-Function4' (LagFilter_sf) */

29039
    {

29040
      SimStruct *rts = ssGetSFunction(S, 120);

29041
      if (rts != (NULL) ) {

29042
        rt_FREE(ssGetSampleTimePtr(rts));

29043
        rt_FREE(ssGetOffsetTimePtr(rts));

29044
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29045


29046
        /* Destroy model methods 3 */

29047
        {

29048
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29049
          rt_FREE(methods3);

29050
        }

29051


29052
        /* Destroy model methods 2 */

29053
        {

29054
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29055
          rt_FREE(methods2);

29056
        }

29057


29058
        /* Destroy state auxilliary information */

29059
        {

29060
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29061
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29062
          rt_FREE(periodicStatesInfo);

29063
          rt_FREE(statesInfo2);

29064
        }

29065


29066
        {

29067
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29068
          rt_FREE(ptr);

29069
        }

29070


29071
        {

29072
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29073
          rt_FREE(ptr);

29074
        }

29075


29076
        {

29077
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29078
          rt_FREE(ptr);

29079
        }

29080


29081
        {

29082
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29083
          rt_FREE(ptr);

29084
        }

29085


29086
        {

29087
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29088
          rt_FREE(ptr);

29089
        }

29090


29091
        {

29092
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

29093
          rt_FREE(ptr);

29094
        }

29095


29096
        rt_FREE(ssGetPortInfoForInputs(rts));

29097
        rt_FREE(ssGetPortInfoForOutputs(rts));

29098
        rt_FREE(ssGetSFcnDWork(rts));

29099
      }

29100
    }

29101


29102
    /* RTW Generated Level2 S-Function Block: '<S209>/Generated S-Function1' (SaturationLimiter_sf) */

29103
    {

29104
      SimStruct *rts = ssGetSFunction(S, 121);

29105
      if (rts != (NULL) ) {

29106
        rt_FREE(ssGetSampleTimePtr(rts));

29107
        rt_FREE(ssGetOffsetTimePtr(rts));

29108
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29109


29110
        /* Destroy model methods 3 */

29111
        {

29112
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29113
          rt_FREE(methods3);

29114
        }

29115


29116
        /* Destroy model methods 2 */

29117
        {

29118
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29119
          rt_FREE(methods2);

29120
        }

29121


29122
        /* Destroy state auxilliary information */

29123
        {

29124
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29125
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29126
          rt_FREE(periodicStatesInfo);

29127
          rt_FREE(statesInfo2);

29128
        }

29129


29130
        {

29131
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29132
          rt_FREE(ptr);

29133
        }

29134


29135
        {

29136
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29137
          rt_FREE(ptr);

29138
        }

29139


29140
        {

29141
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29142
          rt_FREE(ptr);

29143
        }

29144


29145
        rt_FREE(ssGetPortInfoForInputs(rts));

29146
        rt_FREE(ssGetPortInfoForOutputs(rts));

29147
      }

29148
    }

29149


29150
    /* RTW Generated Level2 S-Function Block: '<S208>/Generated S-Function' (RateLimiter_sf) */

29151
    {

29152
      SimStruct *rts = ssGetSFunction(S, 122);

29153
      if (rts != (NULL) ) {

29154
        rt_FREE(ssGetSampleTimePtr(rts));

29155
        rt_FREE(ssGetOffsetTimePtr(rts));

29156
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29157


29158
        /* Destroy model methods 3 */

29159
        {

29160
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29161
          rt_FREE(methods3);

29162
        }

29163


29164
        /* Destroy model methods 2 */

29165
        {

29166
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29167
          rt_FREE(methods2);

29168
        }

29169


29170
        /* Destroy state auxilliary information */

29171
        {

29172
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29173
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29174
          rt_FREE(periodicStatesInfo);

29175
          rt_FREE(statesInfo2);

29176
        }

29177


29178
        {

29179
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29180
          rt_FREE(ptr);

29181
        }

29182


29183
        {

29184
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29185
          rt_FREE(ptr);

29186
        }

29187


29188
        {

29189
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29190
          rt_FREE(ptr);

29191
        }

29192


29193
        {

29194
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29195
          rt_FREE(ptr);

29196
        }

29197


29198
        {

29199
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29200
          rt_FREE(ptr);

29201
        }

29202


29203
        rt_FREE(ssGetPortInfoForInputs(rts));

29204
        rt_FREE(ssGetPortInfoForOutputs(rts));

29205
        rt_FREE(ssGetSFcnDWork(rts));

29206
      }

29207
    }

29208


29209
    /* RTW Generated Level2 S-Function Block: '<S241>/Generated S-Function4' (LagFilter_sf) */

29210
    {

29211
      SimStruct *rts = ssGetSFunction(S, 123);

29212
      if (rts != (NULL) ) {

29213
        rt_FREE(ssGetSampleTimePtr(rts));

29214
        rt_FREE(ssGetOffsetTimePtr(rts));

29215
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29216


29217
        /* Destroy model methods 3 */

29218
        {

29219
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29220
          rt_FREE(methods3);

29221
        }

29222


29223
        /* Destroy model methods 2 */

29224
        {

29225
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29226
          rt_FREE(methods2);

29227
        }

29228


29229
        /* Destroy state auxilliary information */

29230
        {

29231
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29232
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29233
          rt_FREE(periodicStatesInfo);

29234
          rt_FREE(statesInfo2);

29235
        }

29236


29237
        {

29238
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29239
          rt_FREE(ptr);

29240
        }

29241


29242
        {

29243
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29244
          rt_FREE(ptr);

29245
        }

29246


29247
        {

29248
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29249
          rt_FREE(ptr);

29250
        }

29251


29252
        {

29253
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29254
          rt_FREE(ptr);

29255
        }

29256


29257
        {

29258
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29259
          rt_FREE(ptr);

29260
        }

29261


29262
        {

29263
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

29264
          rt_FREE(ptr);

29265
        }

29266


29267
        rt_FREE(ssGetPortInfoForInputs(rts));

29268
        rt_FREE(ssGetPortInfoForOutputs(rts));

29269
        rt_FREE(ssGetSFcnDWork(rts));

29270
      }

29271
    }

29272


29273
    /* RTW Generated Level2 S-Function Block: '<S245>/Generated S-Function' (Gain_sf) */

29274
    {

29275
      SimStruct *rts = ssGetSFunction(S, 124);

29276
      if (rts != (NULL) ) {

29277
        rt_FREE(ssGetSampleTimePtr(rts));

29278
        rt_FREE(ssGetOffsetTimePtr(rts));

29279
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29280


29281
        /* Destroy model methods 3 */

29282
        {

29283
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29284
          rt_FREE(methods3);

29285
        }

29286


29287
        /* Destroy model methods 2 */

29288
        {

29289
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29290
          rt_FREE(methods2);

29291
        }

29292


29293
        /* Destroy state auxilliary information */

29294
        {

29295
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29296
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29297
          rt_FREE(periodicStatesInfo);

29298
          rt_FREE(statesInfo2);

29299
        }

29300


29301
        {

29302
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29303
          rt_FREE(ptr);

29304
        }

29305


29306
        {

29307
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29308
          rt_FREE(ptr);

29309
        }

29310


29311
        rt_FREE(ssGetPortInfoForInputs(rts));

29312
        rt_FREE(ssGetPortInfoForOutputs(rts));

29313
      }

29314
    }

29315


29316
    /* RTW Generated Level2 S-Function Block: '<S246>/Generated S-Function' (Gain_sf) */

29317
    {

29318
      SimStruct *rts = ssGetSFunction(S, 125);

29319
      if (rts != (NULL) ) {

29320
        rt_FREE(ssGetSampleTimePtr(rts));

29321
        rt_FREE(ssGetOffsetTimePtr(rts));

29322
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29323


29324
        /* Destroy model methods 3 */

29325
        {

29326
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29327
          rt_FREE(methods3);

29328
        }

29329


29330
        /* Destroy model methods 2 */

29331
        {

29332
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29333
          rt_FREE(methods2);

29334
        }

29335


29336
        /* Destroy state auxilliary information */

29337
        {

29338
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29339
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29340
          rt_FREE(periodicStatesInfo);

29341
          rt_FREE(statesInfo2);

29342
        }

29343


29344
        {

29345
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29346
          rt_FREE(ptr);

29347
        }

29348


29349
        {

29350
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29351
          rt_FREE(ptr);

29352
        }

29353


29354
        rt_FREE(ssGetPortInfoForInputs(rts));

29355
        rt_FREE(ssGetPortInfoForOutputs(rts));

29356
      }

29357
    }

29358


29359
    /* RTW Generated Level2 S-Function Block: '<S239>/Generated S-Function4' (LagFilter_sf) */

29360
    {

29361
      SimStruct *rts = ssGetSFunction(S, 126);

29362
      if (rts != (NULL) ) {

29363
        rt_FREE(ssGetSampleTimePtr(rts));

29364
        rt_FREE(ssGetOffsetTimePtr(rts));

29365
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29366


29367
        /* Destroy model methods 3 */

29368
        {

29369
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29370
          rt_FREE(methods3);

29371
        }

29372


29373
        /* Destroy model methods 2 */

29374
        {

29375
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29376
          rt_FREE(methods2);

29377
        }

29378


29379
        /* Destroy state auxilliary information */

29380
        {

29381
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29382
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29383
          rt_FREE(periodicStatesInfo);

29384
          rt_FREE(statesInfo2);

29385
        }

29386


29387
        {

29388
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29389
          rt_FREE(ptr);

29390
        }

29391


29392
        {

29393
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29394
          rt_FREE(ptr);

29395
        }

29396


29397
        {

29398
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29399
          rt_FREE(ptr);

29400
        }

29401


29402
        {

29403
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29404
          rt_FREE(ptr);

29405
        }

29406


29407
        {

29408
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29409
          rt_FREE(ptr);

29410
        }

29411


29412
        {

29413
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

29414
          rt_FREE(ptr);

29415
        }

29416


29417
        rt_FREE(ssGetPortInfoForInputs(rts));

29418
        rt_FREE(ssGetPortInfoForOutputs(rts));

29419
        rt_FREE(ssGetSFcnDWork(rts));

29420
      }

29421
    }

29422


29423
    /* RTW Generated Level2 S-Function Block: '<S242>/Generated S-Function' (Gain_sf) */

29424
    {

29425
      SimStruct *rts = ssGetSFunction(S, 127);

29426
      if (rts != (NULL) ) {

29427
        rt_FREE(ssGetSampleTimePtr(rts));

29428
        rt_FREE(ssGetOffsetTimePtr(rts));

29429
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29430


29431
        /* Destroy model methods 3 */

29432
        {

29433
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29434
          rt_FREE(methods3);

29435
        }

29436


29437
        /* Destroy model methods 2 */

29438
        {

29439
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29440
          rt_FREE(methods2);

29441
        }

29442


29443
        /* Destroy state auxilliary information */

29444
        {

29445
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29446
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29447
          rt_FREE(periodicStatesInfo);

29448
          rt_FREE(statesInfo2);

29449
        }

29450


29451
        {

29452
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29453
          rt_FREE(ptr);

29454
        }

29455


29456
        {

29457
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29458
          rt_FREE(ptr);

29459
        }

29460


29461
        rt_FREE(ssGetPortInfoForInputs(rts));

29462
        rt_FREE(ssGetPortInfoForOutputs(rts));

29463
      }

29464
    }

29465


29466
    /* RTW Generated Level2 S-Function Block: '<S240>/Generated S-Function4' (LagFilter_sf) */

29467
    {

29468
      SimStruct *rts = ssGetSFunction(S, 128);

29469
      if (rts != (NULL) ) {

29470
        rt_FREE(ssGetSampleTimePtr(rts));

29471
        rt_FREE(ssGetOffsetTimePtr(rts));

29472
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29473


29474
        /* Destroy model methods 3 */

29475
        {

29476
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29477
          rt_FREE(methods3);

29478
        }

29479


29480
        /* Destroy model methods 2 */

29481
        {

29482
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29483
          rt_FREE(methods2);

29484
        }

29485


29486
        /* Destroy state auxilliary information */

29487
        {

29488
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29489
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29490
          rt_FREE(periodicStatesInfo);

29491
          rt_FREE(statesInfo2);

29492
        }

29493


29494
        {

29495
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29496
          rt_FREE(ptr);

29497
        }

29498


29499
        {

29500
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29501
          rt_FREE(ptr);

29502
        }

29503


29504
        {

29505
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29506
          rt_FREE(ptr);

29507
        }

29508


29509
        {

29510
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29511
          rt_FREE(ptr);

29512
        }

29513


29514
        {

29515
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29516
          rt_FREE(ptr);

29517
        }

29518


29519
        {

29520
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

29521
          rt_FREE(ptr);

29522
        }

29523


29524
        rt_FREE(ssGetPortInfoForInputs(rts));

29525
        rt_FREE(ssGetPortInfoForOutputs(rts));

29526
        rt_FREE(ssGetSFcnDWork(rts));

29527
      }

29528
    }

29529


29530
    /* RTW Generated Level2 S-Function Block: '<S238>/Generated S-Function8' (WashoutFilter_sf) */

29531
    {

29532
      SimStruct *rts = ssGetSFunction(S, 129);

29533
      if (rts != (NULL) ) {

29534
        rt_FREE(ssGetSampleTimePtr(rts));

29535
        rt_FREE(ssGetOffsetTimePtr(rts));

29536
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29537


29538
        /* Destroy model methods 3 */

29539
        {

29540
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29541
          rt_FREE(methods3);

29542
        }

29543


29544
        /* Destroy model methods 2 */

29545
        {

29546
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29547
          rt_FREE(methods2);

29548
        }

29549


29550
        /* Destroy state auxilliary information */

29551
        {

29552
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29553
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29554
          rt_FREE(periodicStatesInfo);

29555
          rt_FREE(statesInfo2);

29556
        }

29557


29558
        {

29559
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29560
          rt_FREE(ptr);

29561
        }

29562


29563
        {

29564
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29565
          rt_FREE(ptr);

29566
        }

29567


29568
        {

29569
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29570
          rt_FREE(ptr);

29571
        }

29572


29573
        {

29574
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29575
          rt_FREE(ptr);

29576
        }

29577


29578
        {

29579
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29580
          rt_FREE(ptr);

29581
        }

29582


29583
        {

29584
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

29585
          rt_FREE(ptr);

29586
        }

29587


29588
        rt_FREE(ssGetPortInfoForInputs(rts));

29589
        rt_FREE(ssGetPortInfoForOutputs(rts));

29590
        rt_FREE(ssGetSFcnDWork(rts));

29591
      }

29592
    }

29593


29594
    /* RTW Generated Level2 S-Function Block: '<S244>/Generated S-Function' (Gain_sf) */

29595
    {

29596
      SimStruct *rts = ssGetSFunction(S, 130);

29597
      if (rts != (NULL) ) {

29598
        rt_FREE(ssGetSampleTimePtr(rts));

29599
        rt_FREE(ssGetOffsetTimePtr(rts));

29600
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29601


29602
        /* Destroy model methods 3 */

29603
        {

29604
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29605
          rt_FREE(methods3);

29606
        }

29607


29608
        /* Destroy model methods 2 */

29609
        {

29610
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29611
          rt_FREE(methods2);

29612
        }

29613


29614
        /* Destroy state auxilliary information */

29615
        {

29616
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29617
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29618
          rt_FREE(periodicStatesInfo);

29619
          rt_FREE(statesInfo2);

29620
        }

29621


29622
        {

29623
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29624
          rt_FREE(ptr);

29625
        }

29626


29627
        {

29628
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29629
          rt_FREE(ptr);

29630
        }

29631


29632
        rt_FREE(ssGetPortInfoForInputs(rts));

29633
        rt_FREE(ssGetPortInfoForOutputs(rts));

29634
      }

29635
    }

29636


29637
    /* RTW Generated Level2 S-Function Block: '<S243>/Generated S-Function' (Gain_sf) */

29638
    {

29639
      SimStruct *rts = ssGetSFunction(S, 131);

29640
      if (rts != (NULL) ) {

29641
        rt_FREE(ssGetSampleTimePtr(rts));

29642
        rt_FREE(ssGetOffsetTimePtr(rts));

29643
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29644


29645
        /* Destroy model methods 3 */

29646
        {

29647
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29648
          rt_FREE(methods3);

29649
        }

29650


29651
        /* Destroy model methods 2 */

29652
        {

29653
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29654
          rt_FREE(methods2);

29655
        }

29656


29657
        /* Destroy state auxilliary information */

29658
        {

29659
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29660
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29661
          rt_FREE(periodicStatesInfo);

29662
          rt_FREE(statesInfo2);

29663
        }

29664


29665
        {

29666
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29667
          rt_FREE(ptr);

29668
        }

29669


29670
        {

29671
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29672
          rt_FREE(ptr);

29673
        }

29674


29675
        rt_FREE(ssGetPortInfoForInputs(rts));

29676
        rt_FREE(ssGetPortInfoForOutputs(rts));

29677
      }

29678
    }

29679


29680
    /* RTW Generated Level2 S-Function Block: '<S225>/Generated S-Function1' (SaturationLimiter_sf) */

29681
    {

29682
      SimStruct *rts = ssGetSFunction(S, 132);

29683
      if (rts != (NULL) ) {

29684
        rt_FREE(ssGetSampleTimePtr(rts));

29685
        rt_FREE(ssGetOffsetTimePtr(rts));

29686
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29687


29688
        /* Destroy model methods 3 */

29689
        {

29690
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29691
          rt_FREE(methods3);

29692
        }

29693


29694
        /* Destroy model methods 2 */

29695
        {

29696
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29697
          rt_FREE(methods2);

29698
        }

29699


29700
        /* Destroy state auxilliary information */

29701
        {

29702
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29703
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29704
          rt_FREE(periodicStatesInfo);

29705
          rt_FREE(statesInfo2);

29706
        }

29707


29708
        {

29709
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29710
          rt_FREE(ptr);

29711
        }

29712


29713
        {

29714
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29715
          rt_FREE(ptr);

29716
        }

29717


29718
        {

29719
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29720
          rt_FREE(ptr);

29721
        }

29722


29723
        rt_FREE(ssGetPortInfoForInputs(rts));

29724
        rt_FREE(ssGetPortInfoForOutputs(rts));

29725
      }

29726
    }

29727


29728
    /* RTW Generated Level2 S-Function Block: '<S223>/Generated S-Function7' (RateLimiter_sf) */

29729
    {

29730
      SimStruct *rts = ssGetSFunction(S, 133);

29731
      if (rts != (NULL) ) {

29732
        rt_FREE(ssGetSampleTimePtr(rts));

29733
        rt_FREE(ssGetOffsetTimePtr(rts));

29734
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29735


29736
        /* Destroy model methods 3 */

29737
        {

29738
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29739
          rt_FREE(methods3);

29740
        }

29741


29742
        /* Destroy model methods 2 */

29743
        {

29744
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29745
          rt_FREE(methods2);

29746
        }

29747


29748
        /* Destroy state auxilliary information */

29749
        {

29750
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29751
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29752
          rt_FREE(periodicStatesInfo);

29753
          rt_FREE(statesInfo2);

29754
        }

29755


29756
        {

29757
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29758
          rt_FREE(ptr);

29759
        }

29760


29761
        {

29762
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29763
          rt_FREE(ptr);

29764
        }

29765


29766
        {

29767
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29768
          rt_FREE(ptr);

29769
        }

29770


29771
        {

29772
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29773
          rt_FREE(ptr);

29774
        }

29775


29776
        {

29777
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29778
          rt_FREE(ptr);

29779
        }

29780


29781
        rt_FREE(ssGetPortInfoForInputs(rts));

29782
        rt_FREE(ssGetPortInfoForOutputs(rts));

29783
        rt_FREE(ssGetSFcnDWork(rts));

29784
      }

29785
    }

29786


29787
    /* RTW Generated Level2 S-Function Block: '<S226>/Generated S-Function1' (SaturationLimiter_sf) */

29788
    {

29789
      SimStruct *rts = ssGetSFunction(S, 134);

29790
      if (rts != (NULL) ) {

29791
        rt_FREE(ssGetSampleTimePtr(rts));

29792
        rt_FREE(ssGetOffsetTimePtr(rts));

29793
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29794


29795
        /* Destroy model methods 3 */

29796
        {

29797
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29798
          rt_FREE(methods3);

29799
        }

29800


29801
        /* Destroy model methods 2 */

29802
        {

29803
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29804
          rt_FREE(methods2);

29805
        }

29806


29807
        /* Destroy state auxilliary information */

29808
        {

29809
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29810
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29811
          rt_FREE(periodicStatesInfo);

29812
          rt_FREE(statesInfo2);

29813
        }

29814


29815
        {

29816
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29817
          rt_FREE(ptr);

29818
        }

29819


29820
        {

29821
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29822
          rt_FREE(ptr);

29823
        }

29824


29825
        {

29826
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29827
          rt_FREE(ptr);

29828
        }

29829


29830
        rt_FREE(ssGetPortInfoForInputs(rts));

29831
        rt_FREE(ssGetPortInfoForOutputs(rts));

29832
      }

29833
    }

29834


29835
    /* RTW Generated Level2 S-Function Block: '<S224>/Generated S-Function7' (RateLimiter_sf) */

29836
    {

29837
      SimStruct *rts = ssGetSFunction(S, 135);

29838
      if (rts != (NULL) ) {

29839
        rt_FREE(ssGetSampleTimePtr(rts));

29840
        rt_FREE(ssGetOffsetTimePtr(rts));

29841
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29842


29843
        /* Destroy model methods 3 */

29844
        {

29845
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29846
          rt_FREE(methods3);

29847
        }

29848


29849
        /* Destroy model methods 2 */

29850
        {

29851
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29852
          rt_FREE(methods2);

29853
        }

29854


29855
        /* Destroy state auxilliary information */

29856
        {

29857
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29858
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29859
          rt_FREE(periodicStatesInfo);

29860
          rt_FREE(statesInfo2);

29861
        }

29862


29863
        {

29864
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29865
          rt_FREE(ptr);

29866
        }

29867


29868
        {

29869
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29870
          rt_FREE(ptr);

29871
        }

29872


29873
        {

29874
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29875
          rt_FREE(ptr);

29876
        }

29877


29878
        {

29879
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29880
          rt_FREE(ptr);

29881
        }

29882


29883
        {

29884
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29885
          rt_FREE(ptr);

29886
        }

29887


29888
        rt_FREE(ssGetPortInfoForInputs(rts));

29889
        rt_FREE(ssGetPortInfoForOutputs(rts));

29890
        rt_FREE(ssGetSFcnDWork(rts));

29891
      }

29892
    }

29893


29894
    /* RTW Generated Level2 S-Function Block: '<S252>/Generated S-Function8' (WashoutFilter_sf) */

29895
    {

29896
      SimStruct *rts = ssGetSFunction(S, 136);

29897
      if (rts != (NULL) ) {

29898
        rt_FREE(ssGetSampleTimePtr(rts));

29899
        rt_FREE(ssGetOffsetTimePtr(rts));

29900
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29901


29902
        /* Destroy model methods 3 */

29903
        {

29904
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29905
          rt_FREE(methods3);

29906
        }

29907


29908
        /* Destroy model methods 2 */

29909
        {

29910
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29911
          rt_FREE(methods2);

29912
        }

29913


29914
        /* Destroy state auxilliary information */

29915
        {

29916
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29917
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29918
          rt_FREE(periodicStatesInfo);

29919
          rt_FREE(statesInfo2);

29920
        }

29921


29922
        {

29923
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29924
          rt_FREE(ptr);

29925
        }

29926


29927
        {

29928
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29929
          rt_FREE(ptr);

29930
        }

29931


29932
        {

29933
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29934
          rt_FREE(ptr);

29935
        }

29936


29937
        {

29938
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

29939
          rt_FREE(ptr);

29940
        }

29941


29942
        {

29943
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

29944
          rt_FREE(ptr);

29945
        }

29946


29947
        {

29948
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

29949
          rt_FREE(ptr);

29950
        }

29951


29952
        rt_FREE(ssGetPortInfoForInputs(rts));

29953
        rt_FREE(ssGetPortInfoForOutputs(rts));

29954
        rt_FREE(ssGetSFcnDWork(rts));

29955
      }

29956
    }

29957


29958
    /* RTW Generated Level2 S-Function Block: '<S253>/Generated S-Function4' (LagFilter_sf) */

29959
    {

29960
      SimStruct *rts = ssGetSFunction(S, 137);

29961
      if (rts != (NULL) ) {

29962
        rt_FREE(ssGetSampleTimePtr(rts));

29963
        rt_FREE(ssGetOffsetTimePtr(rts));

29964
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

29965


29966
        /* Destroy model methods 3 */

29967
        {

29968
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

29969
          rt_FREE(methods3);

29970
        }

29971


29972
        /* Destroy model methods 2 */

29973
        {

29974
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

29975
          rt_FREE(methods2);

29976
        }

29977


29978
        /* Destroy state auxilliary information */

29979
        {

29980
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

29981
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

29982
          rt_FREE(periodicStatesInfo);

29983
          rt_FREE(statesInfo2);

29984
        }

29985


29986
        {

29987
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

29988
          rt_FREE(ptr);

29989
        }

29990


29991
        {

29992
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

29993
          rt_FREE(ptr);

29994
        }

29995


29996
        {

29997
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

29998
          rt_FREE(ptr);

29999
        }

30000


30001
        {

30002
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

30003
          rt_FREE(ptr);

30004
        }

30005


30006
        {

30007
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

30008
          rt_FREE(ptr);

30009
        }

30010


30011
        {

30012
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

30013
          rt_FREE(ptr);

30014
        }

30015


30016
        rt_FREE(ssGetPortInfoForInputs(rts));

30017
        rt_FREE(ssGetPortInfoForOutputs(rts));

30018
        rt_FREE(ssGetSFcnDWork(rts));

30019
      }

30020
    }

30021


30022
    /* RTW Generated Level2 S-Function Block: '<S254>/Generated S-Function4' (LagFilter_sf) */

30023
    {

30024
      SimStruct *rts = ssGetSFunction(S, 138);

30025
      if (rts != (NULL) ) {

30026
        rt_FREE(ssGetSampleTimePtr(rts));

30027
        rt_FREE(ssGetOffsetTimePtr(rts));

30028
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30029


30030
        /* Destroy model methods 3 */

30031
        {

30032
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30033
          rt_FREE(methods3);

30034
        }

30035


30036
        /* Destroy model methods 2 */

30037
        {

30038
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30039
          rt_FREE(methods2);

30040
        }

30041


30042
        /* Destroy state auxilliary information */

30043
        {

30044
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30045
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30046
          rt_FREE(periodicStatesInfo);

30047
          rt_FREE(statesInfo2);

30048
        }

30049


30050
        {

30051
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30052
          rt_FREE(ptr);

30053
        }

30054


30055
        {

30056
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30057
          rt_FREE(ptr);

30058
        }

30059


30060
        {

30061
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

30062
          rt_FREE(ptr);

30063
        }

30064


30065
        {

30066
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

30067
          rt_FREE(ptr);

30068
        }

30069


30070
        {

30071
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

30072
          rt_FREE(ptr);

30073
        }

30074


30075
        {

30076
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

30077
          rt_FREE(ptr);

30078
        }

30079


30080
        rt_FREE(ssGetPortInfoForInputs(rts));

30081
        rt_FREE(ssGetPortInfoForOutputs(rts));

30082
        rt_FREE(ssGetSFcnDWork(rts));

30083
      }

30084
    }

30085


30086
    /* RTW Generated Level2 S-Function Block: '<S259>/Generated S-Function' (Gain_sf) */

30087
    {

30088
      SimStruct *rts = ssGetSFunction(S, 139);

30089
      if (rts != (NULL) ) {

30090
        rt_FREE(ssGetSampleTimePtr(rts));

30091
        rt_FREE(ssGetOffsetTimePtr(rts));

30092
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30093


30094
        /* Destroy model methods 3 */

30095
        {

30096
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30097
          rt_FREE(methods3);

30098
        }

30099


30100
        /* Destroy model methods 2 */

30101
        {

30102
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30103
          rt_FREE(methods2);

30104
        }

30105


30106
        /* Destroy state auxilliary information */

30107
        {

30108
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30109
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30110
          rt_FREE(periodicStatesInfo);

30111
          rt_FREE(statesInfo2);

30112
        }

30113


30114
        {

30115
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30116
          rt_FREE(ptr);

30117
        }

30118


30119
        {

30120
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30121
          rt_FREE(ptr);

30122
        }

30123


30124
        rt_FREE(ssGetPortInfoForInputs(rts));

30125
        rt_FREE(ssGetPortInfoForOutputs(rts));

30126
      }

30127
    }

30128


30129
    /* RTW Generated Level2 S-Function Block: '<S260>/Generated S-Function' (Gain_sf) */

30130
    {

30131
      SimStruct *rts = ssGetSFunction(S, 140);

30132
      if (rts != (NULL) ) {

30133
        rt_FREE(ssGetSampleTimePtr(rts));

30134
        rt_FREE(ssGetOffsetTimePtr(rts));

30135
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30136


30137
        /* Destroy model methods 3 */

30138
        {

30139
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30140
          rt_FREE(methods3);

30141
        }

30142


30143
        /* Destroy model methods 2 */

30144
        {

30145
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30146
          rt_FREE(methods2);

30147
        }

30148


30149
        /* Destroy state auxilliary information */

30150
        {

30151
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30152
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30153
          rt_FREE(periodicStatesInfo);

30154
          rt_FREE(statesInfo2);

30155
        }

30156


30157
        {

30158
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30159
          rt_FREE(ptr);

30160
        }

30161


30162
        {

30163
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30164
          rt_FREE(ptr);

30165
        }

30166


30167
        rt_FREE(ssGetPortInfoForInputs(rts));

30168
        rt_FREE(ssGetPortInfoForOutputs(rts));

30169
      }

30170
    }

30171


30172
    /* RTW Generated Level2 S-Function Block: '<S255>/Generated S-Function9' (NegativePass_sf) */

30173
    {

30174
      SimStruct *rts = ssGetSFunction(S, 141);

30175
      if (rts != (NULL) ) {

30176
        rt_FREE(ssGetSampleTimePtr(rts));

30177
        rt_FREE(ssGetOffsetTimePtr(rts));

30178
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30179


30180
        /* Destroy model methods 3 */

30181
        {

30182
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30183
          rt_FREE(methods3);

30184
        }

30185


30186
        /* Destroy model methods 2 */

30187
        {

30188
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30189
          rt_FREE(methods2);

30190
        }

30191


30192
        /* Destroy state auxilliary information */

30193
        {

30194
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30195
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30196
          rt_FREE(periodicStatesInfo);

30197
          rt_FREE(statesInfo2);

30198
        }

30199


30200
        {

30201
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30202
          rt_FREE(ptr);

30203
        }

30204


30205
        rt_FREE(ssGetPortInfoForInputs(rts));

30206
        rt_FREE(ssGetPortInfoForOutputs(rts));

30207
      }

30208
    }

30209


30210
    /* RTW Generated Level2 S-Function Block: '<S266>/Generated S-Function' (WashoutFilter_sf) */

30211
    {

30212
      SimStruct *rts = ssGetSFunction(S, 142);

30213
      if (rts != (NULL) ) {

30214
        rt_FREE(ssGetSampleTimePtr(rts));

30215
        rt_FREE(ssGetOffsetTimePtr(rts));

30216
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30217


30218
        /* Destroy model methods 3 */

30219
        {

30220
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30221
          rt_FREE(methods3);

30222
        }

30223


30224
        /* Destroy model methods 2 */

30225
        {

30226
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30227
          rt_FREE(methods2);

30228
        }

30229


30230
        /* Destroy state auxilliary information */

30231
        {

30232
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30233
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30234
          rt_FREE(periodicStatesInfo);

30235
          rt_FREE(statesInfo2);

30236
        }

30237


30238
        {

30239
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30240
          rt_FREE(ptr);

30241
        }

30242


30243
        {

30244
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30245
          rt_FREE(ptr);

30246
        }

30247


30248
        {

30249
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

30250
          rt_FREE(ptr);

30251
        }

30252


30253
        {

30254
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

30255
          rt_FREE(ptr);

30256
        }

30257


30258
        {

30259
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

30260
          rt_FREE(ptr);

30261
        }

30262


30263
        {

30264
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

30265
          rt_FREE(ptr);

30266
        }

30267


30268
        rt_FREE(ssGetPortInfoForInputs(rts));

30269
        rt_FREE(ssGetPortInfoForOutputs(rts));

30270
        rt_FREE(ssGetSFcnDWork(rts));

30271
      }

30272
    }

30273


30274
    /* RTW Generated Level2 S-Function Block: '<S268>/Generated S-Function' (Gain_sf) */

30275
    {

30276
      SimStruct *rts = ssGetSFunction(S, 143);

30277
      if (rts != (NULL) ) {

30278
        rt_FREE(ssGetSampleTimePtr(rts));

30279
        rt_FREE(ssGetOffsetTimePtr(rts));

30280
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30281


30282
        /* Destroy model methods 3 */

30283
        {

30284
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30285
          rt_FREE(methods3);

30286
        }

30287


30288
        /* Destroy model methods 2 */

30289
        {

30290
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30291
          rt_FREE(methods2);

30292
        }

30293


30294
        /* Destroy state auxilliary information */

30295
        {

30296
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30297
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30298
          rt_FREE(periodicStatesInfo);

30299
          rt_FREE(statesInfo2);

30300
        }

30301


30302
        {

30303
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30304
          rt_FREE(ptr);

30305
        }

30306


30307
        {

30308
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30309
          rt_FREE(ptr);

30310
        }

30311


30312
        rt_FREE(ssGetPortInfoForInputs(rts));

30313
        rt_FREE(ssGetPortInfoForOutputs(rts));

30314
      }

30315
    }

30316


30317
    /* RTW Generated Level2 S-Function Block: '<S265>/Generated S-Function8' (WashoutFilter_sf) */

30318
    {

30319
      SimStruct *rts = ssGetSFunction(S, 144);

30320
      if (rts != (NULL) ) {

30321
        rt_FREE(ssGetSampleTimePtr(rts));

30322
        rt_FREE(ssGetOffsetTimePtr(rts));

30323
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30324


30325
        /* Destroy model methods 3 */

30326
        {

30327
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30328
          rt_FREE(methods3);

30329
        }

30330


30331
        /* Destroy model methods 2 */

30332
        {

30333
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30334
          rt_FREE(methods2);

30335
        }

30336


30337
        /* Destroy state auxilliary information */

30338
        {

30339
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30340
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30341
          rt_FREE(periodicStatesInfo);

30342
          rt_FREE(statesInfo2);

30343
        }

30344


30345
        {

30346
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30347
          rt_FREE(ptr);

30348
        }

30349


30350
        {

30351
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30352
          rt_FREE(ptr);

30353
        }

30354


30355
        {

30356
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

30357
          rt_FREE(ptr);

30358
        }

30359


30360
        {

30361
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

30362
          rt_FREE(ptr);

30363
        }

30364


30365
        {

30366
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

30367
          rt_FREE(ptr);

30368
        }

30369


30370
        {

30371
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

30372
          rt_FREE(ptr);

30373
        }

30374


30375
        rt_FREE(ssGetPortInfoForInputs(rts));

30376
        rt_FREE(ssGetPortInfoForOutputs(rts));

30377
        rt_FREE(ssGetSFcnDWork(rts));

30378
      }

30379
    }

30380


30381
    /* RTW Generated Level2 S-Function Block: '<S269>/Generated S-Function' (Gain_sf) */

30382
    {

30383
      SimStruct *rts = ssGetSFunction(S, 145);

30384
      if (rts != (NULL) ) {

30385
        rt_FREE(ssGetSampleTimePtr(rts));

30386
        rt_FREE(ssGetOffsetTimePtr(rts));

30387
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30388


30389
        /* Destroy model methods 3 */

30390
        {

30391
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30392
          rt_FREE(methods3);

30393
        }

30394


30395
        /* Destroy model methods 2 */

30396
        {

30397
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30398
          rt_FREE(methods2);

30399
        }

30400


30401
        /* Destroy state auxilliary information */

30402
        {

30403
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30404
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30405
          rt_FREE(periodicStatesInfo);

30406
          rt_FREE(statesInfo2);

30407
        }

30408


30409
        {

30410
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30411
          rt_FREE(ptr);

30412
        }

30413


30414
        {

30415
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30416
          rt_FREE(ptr);

30417
        }

30418


30419
        rt_FREE(ssGetPortInfoForInputs(rts));

30420
        rt_FREE(ssGetPortInfoForOutputs(rts));

30421
      }

30422
    }

30423


30424
    /* RTW Generated Level2 S-Function Block: '<S267>/Generated S-Function' (NegativePass_sf) */

30425
    {

30426
      SimStruct *rts = ssGetSFunction(S, 146);

30427
      if (rts != (NULL) ) {

30428
        rt_FREE(ssGetSampleTimePtr(rts));

30429
        rt_FREE(ssGetOffsetTimePtr(rts));

30430
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30431


30432
        /* Destroy model methods 3 */

30433
        {

30434
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30435
          rt_FREE(methods3);

30436
        }

30437


30438
        /* Destroy model methods 2 */

30439
        {

30440
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30441
          rt_FREE(methods2);

30442
        }

30443


30444
        /* Destroy state auxilliary information */

30445
        {

30446
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30447
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30448
          rt_FREE(periodicStatesInfo);

30449
          rt_FREE(statesInfo2);

30450
        }

30451


30452
        {

30453
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30454
          rt_FREE(ptr);

30455
        }

30456


30457
        rt_FREE(ssGetPortInfoForInputs(rts));

30458
        rt_FREE(ssGetPortInfoForOutputs(rts));

30459
      }

30460
    }

30461


30462
    /* RTW Generated Level2 S-Function Block: '<S270>/Generated S-Function' (Gain_sf) */

30463
    {

30464
      SimStruct *rts = ssGetSFunction(S, 147);

30465
      if (rts != (NULL) ) {

30466
        rt_FREE(ssGetSampleTimePtr(rts));

30467
        rt_FREE(ssGetOffsetTimePtr(rts));

30468
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30469


30470
        /* Destroy model methods 3 */

30471
        {

30472
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30473
          rt_FREE(methods3);

30474
        }

30475


30476
        /* Destroy model methods 2 */

30477
        {

30478
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30479
          rt_FREE(methods2);

30480
        }

30481


30482
        /* Destroy state auxilliary information */

30483
        {

30484
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30485
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30486
          rt_FREE(periodicStatesInfo);

30487
          rt_FREE(statesInfo2);

30488
        }

30489


30490
        {

30491
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30492
          rt_FREE(ptr);

30493
        }

30494


30495
        {

30496
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30497
          rt_FREE(ptr);

30498
        }

30499


30500
        rt_FREE(ssGetPortInfoForInputs(rts));

30501
        rt_FREE(ssGetPortInfoForOutputs(rts));

30502
      }

30503
    }

30504


30505
    /* RTW Generated Level2 S-Function Block: '<S272>/Generated S-Function' (PositivePass_sf) */

30506
    {

30507
      SimStruct *rts = ssGetSFunction(S, 148);

30508
      if (rts != (NULL) ) {

30509
        rt_FREE(ssGetSampleTimePtr(rts));

30510
        rt_FREE(ssGetOffsetTimePtr(rts));

30511
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30512


30513
        /* Destroy model methods 3 */

30514
        {

30515
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30516
          rt_FREE(methods3);

30517
        }

30518


30519
        /* Destroy model methods 2 */

30520
        {

30521
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30522
          rt_FREE(methods2);

30523
        }

30524


30525
        /* Destroy state auxilliary information */

30526
        {

30527
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30528
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30529
          rt_FREE(periodicStatesInfo);

30530
          rt_FREE(statesInfo2);

30531
        }

30532


30533
        {

30534
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30535
          rt_FREE(ptr);

30536
        }

30537


30538
        rt_FREE(ssGetPortInfoForInputs(rts));

30539
        rt_FREE(ssGetPortInfoForOutputs(rts));

30540
      }

30541
    }

30542


30543
    /* RTW Generated Level2 S-Function Block: '<S271>/Generated S-Function' (Gain_sf) */

30544
    {

30545
      SimStruct *rts = ssGetSFunction(S, 149);

30546
      if (rts != (NULL) ) {

30547
        rt_FREE(ssGetSampleTimePtr(rts));

30548
        rt_FREE(ssGetOffsetTimePtr(rts));

30549
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30550


30551
        /* Destroy model methods 3 */

30552
        {

30553
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30554
          rt_FREE(methods3);

30555
        }

30556


30557
        /* Destroy model methods 2 */

30558
        {

30559
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30560
          rt_FREE(methods2);

30561
        }

30562


30563
        /* Destroy state auxilliary information */

30564
        {

30565
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30566
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30567
          rt_FREE(periodicStatesInfo);

30568
          rt_FREE(statesInfo2);

30569
        }

30570


30571
        {

30572
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30573
          rt_FREE(ptr);

30574
        }

30575


30576
        {

30577
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30578
          rt_FREE(ptr);

30579
        }

30580


30581
        rt_FREE(ssGetPortInfoForInputs(rts));

30582
        rt_FREE(ssGetPortInfoForOutputs(rts));

30583
      }

30584
    }

30585


30586
    /* RTW Generated Level2 S-Function Block: '<S261>/Generated S-Function' (Gain_sf) */

30587
    {

30588
      SimStruct *rts = ssGetSFunction(S, 150);

30589
      if (rts != (NULL) ) {

30590
        rt_FREE(ssGetSampleTimePtr(rts));

30591
        rt_FREE(ssGetOffsetTimePtr(rts));

30592
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30593


30594
        /* Destroy model methods 3 */

30595
        {

30596
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30597
          rt_FREE(methods3);

30598
        }

30599


30600
        /* Destroy model methods 2 */

30601
        {

30602
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30603
          rt_FREE(methods2);

30604
        }

30605


30606
        /* Destroy state auxilliary information */

30607
        {

30608
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30609
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30610
          rt_FREE(periodicStatesInfo);

30611
          rt_FREE(statesInfo2);

30612
        }

30613


30614
        {

30615
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30616
          rt_FREE(ptr);

30617
        }

30618


30619
        {

30620
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30621
          rt_FREE(ptr);

30622
        }

30623


30624
        rt_FREE(ssGetPortInfoForInputs(rts));

30625
        rt_FREE(ssGetPortInfoForOutputs(rts));

30626
      }

30627
    }

30628


30629
    /* RTW Generated Level2 S-Function Block: '<S262>/Generated S-Function' (Gain_sf) */

30630
    {

30631
      SimStruct *rts = ssGetSFunction(S, 151);

30632
      if (rts != (NULL) ) {

30633
        rt_FREE(ssGetSampleTimePtr(rts));

30634
        rt_FREE(ssGetOffsetTimePtr(rts));

30635
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30636


30637
        /* Destroy model methods 3 */

30638
        {

30639
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30640
          rt_FREE(methods3);

30641
        }

30642


30643
        /* Destroy model methods 2 */

30644
        {

30645
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30646
          rt_FREE(methods2);

30647
        }

30648


30649
        /* Destroy state auxilliary information */

30650
        {

30651
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30652
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30653
          rt_FREE(periodicStatesInfo);

30654
          rt_FREE(statesInfo2);

30655
        }

30656


30657
        {

30658
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30659
          rt_FREE(ptr);

30660
        }

30661


30662
        {

30663
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30664
          rt_FREE(ptr);

30665
        }

30666


30667
        rt_FREE(ssGetPortInfoForInputs(rts));

30668
        rt_FREE(ssGetPortInfoForOutputs(rts));

30669
      }

30670
    }

30671


30672
    /* RTW Generated Level2 S-Function Block: '<S258>/Generated S-Function1' (SaturationLimiter_sf) */

30673
    {

30674
      SimStruct *rts = ssGetSFunction(S, 152);

30675
      if (rts != (NULL) ) {

30676
        rt_FREE(ssGetSampleTimePtr(rts));

30677
        rt_FREE(ssGetOffsetTimePtr(rts));

30678
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30679


30680
        /* Destroy model methods 3 */

30681
        {

30682
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30683
          rt_FREE(methods3);

30684
        }

30685


30686
        /* Destroy model methods 2 */

30687
        {

30688
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30689
          rt_FREE(methods2);

30690
        }

30691


30692
        /* Destroy state auxilliary information */

30693
        {

30694
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30695
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30696
          rt_FREE(periodicStatesInfo);

30697
          rt_FREE(statesInfo2);

30698
        }

30699


30700
        {

30701
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30702
          rt_FREE(ptr);

30703
        }

30704


30705
        {

30706
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30707
          rt_FREE(ptr);

30708
        }

30709


30710
        {

30711
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

30712
          rt_FREE(ptr);

30713
        }

30714


30715
        rt_FREE(ssGetPortInfoForInputs(rts));

30716
        rt_FREE(ssGetPortInfoForOutputs(rts));

30717
      }

30718
    }

30719


30720
    /* RTW Generated Level2 S-Function Block: '<S263>/Generated S-Function' (Gain_sf) */

30721
    {

30722
      SimStruct *rts = ssGetSFunction(S, 153);

30723
      if (rts != (NULL) ) {

30724
        rt_FREE(ssGetSampleTimePtr(rts));

30725
        rt_FREE(ssGetOffsetTimePtr(rts));

30726
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30727


30728
        /* Destroy model methods 3 */

30729
        {

30730
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30731
          rt_FREE(methods3);

30732
        }

30733


30734
        /* Destroy model methods 2 */

30735
        {

30736
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30737
          rt_FREE(methods2);

30738
        }

30739


30740
        /* Destroy state auxilliary information */

30741
        {

30742
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30743
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30744
          rt_FREE(periodicStatesInfo);

30745
          rt_FREE(statesInfo2);

30746
        }

30747


30748
        {

30749
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30750
          rt_FREE(ptr);

30751
        }

30752


30753
        {

30754
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30755
          rt_FREE(ptr);

30756
        }

30757


30758
        rt_FREE(ssGetPortInfoForInputs(rts));

30759
        rt_FREE(ssGetPortInfoForOutputs(rts));

30760
      }

30761
    }

30762


30763
    /* RTW Generated Level2 S-Function Block: '<S264>/Generated S-Function' (Gain_sf) */

30764
    {

30765
      SimStruct *rts = ssGetSFunction(S, 154);

30766
      if (rts != (NULL) ) {

30767
        rt_FREE(ssGetSampleTimePtr(rts));

30768
        rt_FREE(ssGetOffsetTimePtr(rts));

30769
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30770


30771
        /* Destroy model methods 3 */

30772
        {

30773
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30774
          rt_FREE(methods3);

30775
        }

30776


30777
        /* Destroy model methods 2 */

30778
        {

30779
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30780
          rt_FREE(methods2);

30781
        }

30782


30783
        /* Destroy state auxilliary information */

30784
        {

30785
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30786
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30787
          rt_FREE(periodicStatesInfo);

30788
          rt_FREE(statesInfo2);

30789
        }

30790


30791
        {

30792
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30793
          rt_FREE(ptr);

30794
        }

30795


30796
        {

30797
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30798
          rt_FREE(ptr);

30799
        }

30800


30801
        rt_FREE(ssGetPortInfoForInputs(rts));

30802
        rt_FREE(ssGetPortInfoForOutputs(rts));

30803
      }

30804
    }

30805


30806
    /* RTW Generated Level2 S-Function Block: '<S257>/Generated S-Function3' (IntegratorLimited_sf) */

30807
    {

30808
      SimStruct *rts = ssGetSFunction(S, 155);

30809
      if (rts != (NULL) ) {

30810
        rt_FREE(ssGetSampleTimePtr(rts));

30811
        rt_FREE(ssGetOffsetTimePtr(rts));

30812
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30813


30814
        /* Destroy model methods 3 */

30815
        {

30816
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30817
          rt_FREE(methods3);

30818
        }

30819


30820
        /* Destroy model methods 2 */

30821
        {

30822
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30823
          rt_FREE(methods2);

30824
        }

30825


30826
        /* Destroy state auxilliary information */

30827
        {

30828
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30829
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30830
          rt_FREE(periodicStatesInfo);

30831
          rt_FREE(statesInfo2);

30832
        }

30833


30834
        {

30835
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30836
          rt_FREE(ptr);

30837
        }

30838


30839
        {

30840
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30841
          rt_FREE(ptr);

30842
        }

30843


30844
        {

30845
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

30846
          rt_FREE(ptr);

30847
        }

30848


30849
        {

30850
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

30851
          rt_FREE(ptr);

30852
        }

30853


30854
        {

30855
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

30856
          rt_FREE(ptr);

30857
        }

30858


30859
        {

30860
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

30861
          rt_FREE(ptr);

30862
        }

30863


30864
        {

30865
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);

30866
          rt_FREE(ptr);

30867
        }

30868


30869
        rt_FREE(ssGetPortInfoForInputs(rts));

30870
        rt_FREE(ssGetPortInfoForOutputs(rts));

30871
        rt_FREE(ssGetSFcnDWork(rts));

30872
      }

30873
    }

30874


30875
    /* RTW Generated Level2 S-Function Block: '<S250>/Generated S-Function1' (SaturationLimiter_sf) */

30876
    {

30877
      SimStruct *rts = ssGetSFunction(S, 156);

30878
      if (rts != (NULL) ) {

30879
        rt_FREE(ssGetSampleTimePtr(rts));

30880
        rt_FREE(ssGetOffsetTimePtr(rts));

30881
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30882


30883
        /* Destroy model methods 3 */

30884
        {

30885
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30886
          rt_FREE(methods3);

30887
        }

30888


30889
        /* Destroy model methods 2 */

30890
        {

30891
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30892
          rt_FREE(methods2);

30893
        }

30894


30895
        /* Destroy state auxilliary information */

30896
        {

30897
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30898
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30899
          rt_FREE(periodicStatesInfo);

30900
          rt_FREE(statesInfo2);

30901
        }

30902


30903
        {

30904
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30905
          rt_FREE(ptr);

30906
        }

30907


30908
        {

30909
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30910
          rt_FREE(ptr);

30911
        }

30912


30913
        {

30914
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

30915
          rt_FREE(ptr);

30916
        }

30917


30918
        rt_FREE(ssGetPortInfoForInputs(rts));

30919
        rt_FREE(ssGetPortInfoForOutputs(rts));

30920
      }

30921
    }

30922


30923
    /* RTW Generated Level2 S-Function Block: '<S249>/Generated S-Function7' (RateLimiter_sf) */

30924
    {

30925
      SimStruct *rts = ssGetSFunction(S, 157);

30926
      if (rts != (NULL) ) {

30927
        rt_FREE(ssGetSampleTimePtr(rts));

30928
        rt_FREE(ssGetOffsetTimePtr(rts));

30929
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30930


30931
        /* Destroy model methods 3 */

30932
        {

30933
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30934
          rt_FREE(methods3);

30935
        }

30936


30937
        /* Destroy model methods 2 */

30938
        {

30939
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30940
          rt_FREE(methods2);

30941
        }

30942


30943
        /* Destroy state auxilliary information */

30944
        {

30945
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

30946
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

30947
          rt_FREE(periodicStatesInfo);

30948
          rt_FREE(statesInfo2);

30949
        }

30950


30951
        {

30952
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

30953
          rt_FREE(ptr);

30954
        }

30955


30956
        {

30957
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

30958
          rt_FREE(ptr);

30959
        }

30960


30961
        {

30962
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

30963
          rt_FREE(ptr);

30964
        }

30965


30966
        {

30967
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

30968
          rt_FREE(ptr);

30969
        }

30970


30971
        {

30972
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

30973
          rt_FREE(ptr);

30974
        }

30975


30976
        rt_FREE(ssGetPortInfoForInputs(rts));

30977
        rt_FREE(ssGetPortInfoForOutputs(rts));

30978
        rt_FREE(ssGetSFcnDWork(rts));

30979
      }

30980
    }

30981


30982
    /* RTW Generated Level2 S-Function Block: '<S275>/Generated S-Function1' (SaturationLimiter_sf) */

30983
    {

30984
      SimStruct *rts = ssGetSFunction(S, 158);

30985
      if (rts != (NULL) ) {

30986
        rt_FREE(ssGetSampleTimePtr(rts));

30987
        rt_FREE(ssGetOffsetTimePtr(rts));

30988
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

30989


30990
        /* Destroy model methods 3 */

30991
        {

30992
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

30993
          rt_FREE(methods3);

30994
        }

30995


30996
        /* Destroy model methods 2 */

30997
        {

30998
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

30999
          rt_FREE(methods2);

31000
        }

31001


31002
        /* Destroy state auxilliary information */

31003
        {

31004
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31005
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31006
          rt_FREE(periodicStatesInfo);

31007
          rt_FREE(statesInfo2);

31008
        }

31009


31010
        {

31011
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31012
          rt_FREE(ptr);

31013
        }

31014


31015
        {

31016
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31017
          rt_FREE(ptr);

31018
        }

31019


31020
        {

31021
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

31022
          rt_FREE(ptr);

31023
        }

31024


31025
        rt_FREE(ssGetPortInfoForInputs(rts));

31026
        rt_FREE(ssGetPortInfoForOutputs(rts));

31027
      }

31028
    }

31029


31030
    /* RTW Generated Level2 S-Function Block: '<S274>/Generated S-Function7' (RateLimiter_sf) */

31031
    {

31032
      SimStruct *rts = ssGetSFunction(S, 159);

31033
      if (rts != (NULL) ) {

31034
        rt_FREE(ssGetSampleTimePtr(rts));

31035
        rt_FREE(ssGetOffsetTimePtr(rts));

31036
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31037


31038
        /* Destroy model methods 3 */

31039
        {

31040
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31041
          rt_FREE(methods3);

31042
        }

31043


31044
        /* Destroy model methods 2 */

31045
        {

31046
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31047
          rt_FREE(methods2);

31048
        }

31049


31050
        /* Destroy state auxilliary information */

31051
        {

31052
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31053
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31054
          rt_FREE(periodicStatesInfo);

31055
          rt_FREE(statesInfo2);

31056
        }

31057


31058
        {

31059
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31060
          rt_FREE(ptr);

31061
        }

31062


31063
        {

31064
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31065
          rt_FREE(ptr);

31066
        }

31067


31068
        {

31069
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

31070
          rt_FREE(ptr);

31071
        }

31072


31073
        {

31074
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

31075
          rt_FREE(ptr);

31076
        }

31077


31078
        {

31079
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

31080
          rt_FREE(ptr);

31081
        }

31082


31083
        rt_FREE(ssGetPortInfoForInputs(rts));

31084
        rt_FREE(ssGetPortInfoForOutputs(rts));

31085
        rt_FREE(ssGetSFcnDWork(rts));

31086
      }

31087
    }

31088


31089
    /* RTW Generated Level2 S-Function Block: '<S215>/Generated S-Function4' (LagFilter_sf) */

31090
    {

31091
      SimStruct *rts = ssGetSFunction(S, 160);

31092
      if (rts != (NULL) ) {

31093
        rt_FREE(ssGetSampleTimePtr(rts));

31094
        rt_FREE(ssGetOffsetTimePtr(rts));

31095
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31096


31097
        /* Destroy model methods 3 */

31098
        {

31099
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31100
          rt_FREE(methods3);

31101
        }

31102


31103
        /* Destroy model methods 2 */

31104
        {

31105
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31106
          rt_FREE(methods2);

31107
        }

31108


31109
        /* Destroy state auxilliary information */

31110
        {

31111
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31112
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31113
          rt_FREE(periodicStatesInfo);

31114
          rt_FREE(statesInfo2);

31115
        }

31116


31117
        {

31118
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31119
          rt_FREE(ptr);

31120
        }

31121


31122
        {

31123
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31124
          rt_FREE(ptr);

31125
        }

31126


31127
        {

31128
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

31129
          rt_FREE(ptr);

31130
        }

31131


31132
        {

31133
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

31134
          rt_FREE(ptr);

31135
        }

31136


31137
        {

31138
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

31139
          rt_FREE(ptr);

31140
        }

31141


31142
        {

31143
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);

31144
          rt_FREE(ptr);

31145
        }

31146


31147
        rt_FREE(ssGetPortInfoForInputs(rts));

31148
        rt_FREE(ssGetPortInfoForOutputs(rts));

31149
        rt_FREE(ssGetSFcnDWork(rts));

31150
      }

31151
    }

31152


31153
    /* RTW Generated Level2 S-Function Block: '<S218>/Generated S-Function' (Gain_sf) */

31154
    {

31155
      SimStruct *rts = ssGetSFunction(S, 161);

31156
      if (rts != (NULL) ) {

31157
        rt_FREE(ssGetSampleTimePtr(rts));

31158
        rt_FREE(ssGetOffsetTimePtr(rts));

31159
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31160


31161
        /* Destroy model methods 3 */

31162
        {

31163
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31164
          rt_FREE(methods3);

31165
        }

31166


31167
        /* Destroy model methods 2 */

31168
        {

31169
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31170
          rt_FREE(methods2);

31171
        }

31172


31173
        /* Destroy state auxilliary information */

31174
        {

31175
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31176
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31177
          rt_FREE(periodicStatesInfo);

31178
          rt_FREE(statesInfo2);

31179
        }

31180


31181
        {

31182
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31183
          rt_FREE(ptr);

31184
        }

31185


31186
        {

31187
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31188
          rt_FREE(ptr);

31189
        }

31190


31191
        rt_FREE(ssGetPortInfoForInputs(rts));

31192
        rt_FREE(ssGetPortInfoForOutputs(rts));

31193
      }

31194
    }

31195


31196
    /* RTW Generated Level2 S-Function Block: '<S217>/Generated S-Function1' (SaturationLimiter_sf) */

31197
    {

31198
      SimStruct *rts = ssGetSFunction(S, 162);

31199
      if (rts != (NULL) ) {

31200
        rt_FREE(ssGetSampleTimePtr(rts));

31201
        rt_FREE(ssGetOffsetTimePtr(rts));

31202
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31203


31204
        /* Destroy model methods 3 */

31205
        {

31206
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31207
          rt_FREE(methods3);

31208
        }

31209


31210
        /* Destroy model methods 2 */

31211
        {

31212
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31213
          rt_FREE(methods2);

31214
        }

31215


31216
        /* Destroy state auxilliary information */

31217
        {

31218
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31219
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31220
          rt_FREE(periodicStatesInfo);

31221
          rt_FREE(statesInfo2);

31222
        }

31223


31224
        {

31225
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31226
          rt_FREE(ptr);

31227
        }

31228


31229
        {

31230
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31231
          rt_FREE(ptr);

31232
        }

31233


31234
        {

31235
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

31236
          rt_FREE(ptr);

31237
        }

31238


31239
        rt_FREE(ssGetPortInfoForInputs(rts));

31240
        rt_FREE(ssGetPortInfoForOutputs(rts));

31241
      }

31242
    }

31243


31244
    /* RTW Generated Level2 S-Function Block: '<S216>/Generated S-Function7' (RateLimiter_sf) */

31245
    {

31246
      SimStruct *rts = ssGetSFunction(S, 163);

31247
      if (rts != (NULL) ) {

31248
        rt_FREE(ssGetSampleTimePtr(rts));

31249
        rt_FREE(ssGetOffsetTimePtr(rts));

31250
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31251


31252
        /* Destroy model methods 3 */

31253
        {

31254
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31255
          rt_FREE(methods3);

31256
        }

31257


31258
        /* Destroy model methods 2 */

31259
        {

31260
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31261
          rt_FREE(methods2);

31262
        }

31263


31264
        /* Destroy state auxilliary information */

31265
        {

31266
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31267
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31268
          rt_FREE(periodicStatesInfo);

31269
          rt_FREE(statesInfo2);

31270
        }

31271


31272
        {

31273
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31274
          rt_FREE(ptr);

31275
        }

31276


31277
        {

31278
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31279
          rt_FREE(ptr);

31280
        }

31281


31282
        {

31283
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

31284
          rt_FREE(ptr);

31285
        }

31286


31287
        {

31288
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

31289
          rt_FREE(ptr);

31290
        }

31291


31292
        {

31293
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);

31294
          rt_FREE(ptr);

31295
        }

31296


31297
        rt_FREE(ssGetPortInfoForInputs(rts));

31298
        rt_FREE(ssGetPortInfoForOutputs(rts));

31299
        rt_FREE(ssGetSFcnDWork(rts));

31300
      }

31301
    }

31302


31303
    /* RTW Generated Level2 S-Function Block: '<S279>/Generated S-Function' (Gain_sf) */

31304
    {

31305
      SimStruct *rts = ssGetSFunction(S, 164);

31306
      if (rts != (NULL) ) {

31307
        rt_FREE(ssGetSampleTimePtr(rts));

31308
        rt_FREE(ssGetOffsetTimePtr(rts));

31309
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31310


31311
        /* Destroy model methods 3 */

31312
        {

31313
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31314
          rt_FREE(methods3);

31315
        }

31316


31317
        /* Destroy model methods 2 */

31318
        {

31319
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31320
          rt_FREE(methods2);

31321
        }

31322


31323
        /* Destroy state auxilliary information */

31324
        {

31325
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31326
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31327
          rt_FREE(periodicStatesInfo);

31328
          rt_FREE(statesInfo2);

31329
        }

31330


31331
        {

31332
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31333
          rt_FREE(ptr);

31334
        }

31335


31336
        {

31337
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31338
          rt_FREE(ptr);

31339
        }

31340


31341
        rt_FREE(ssGetPortInfoForInputs(rts));

31342
        rt_FREE(ssGetPortInfoForOutputs(rts));

31343
      }

31344
    }

31345


31346
    /* RTW Generated Level2 S-Function Block: '<S280>/Generated S-Function' (Gain_sf) */

31347
    {

31348
      SimStruct *rts = ssGetSFunction(S, 165);

31349
      if (rts != (NULL) ) {

31350
        rt_FREE(ssGetSampleTimePtr(rts));

31351
        rt_FREE(ssGetOffsetTimePtr(rts));

31352
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31353


31354
        /* Destroy model methods 3 */

31355
        {

31356
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31357
          rt_FREE(methods3);

31358
        }

31359


31360
        /* Destroy model methods 2 */

31361
        {

31362
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31363
          rt_FREE(methods2);

31364
        }

31365


31366
        /* Destroy state auxilliary information */

31367
        {

31368
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31369
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31370
          rt_FREE(periodicStatesInfo);

31371
          rt_FREE(statesInfo2);

31372
        }

31373


31374
        {

31375
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31376
          rt_FREE(ptr);

31377
        }

31378


31379
        {

31380
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31381
          rt_FREE(ptr);

31382
        }

31383


31384
        rt_FREE(ssGetPortInfoForInputs(rts));

31385
        rt_FREE(ssGetPortInfoForOutputs(rts));

31386
      }

31387
    }

31388


31389
    /* RTW Generated Level2 S-Function Block: '<S281>/Generated S-Function' (Gain_sf) */

31390
    {

31391
      SimStruct *rts = ssGetSFunction(S, 166);

31392
      if (rts != (NULL) ) {

31393
        rt_FREE(ssGetSampleTimePtr(rts));

31394
        rt_FREE(ssGetOffsetTimePtr(rts));

31395
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31396


31397
        /* Destroy model methods 3 */

31398
        {

31399
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31400
          rt_FREE(methods3);

31401
        }

31402


31403
        /* Destroy model methods 2 */

31404
        {

31405
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31406
          rt_FREE(methods2);

31407
        }

31408


31409
        /* Destroy state auxilliary information */

31410
        {

31411
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31412
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31413
          rt_FREE(periodicStatesInfo);

31414
          rt_FREE(statesInfo2);

31415
        }

31416


31417
        {

31418
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31419
          rt_FREE(ptr);

31420
        }

31421


31422
        {

31423
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31424
          rt_FREE(ptr);

31425
        }

31426


31427
        rt_FREE(ssGetPortInfoForInputs(rts));

31428
        rt_FREE(ssGetPortInfoForOutputs(rts));

31429
      }

31430
    }

31431


31432
    /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */

31433
    {

31434
      SimStruct *rts = ssGetSFunction(S, 167);

31435
      if (rts != (NULL) ) {

31436
        rt_FREE(ssGetSampleTimePtr(rts));

31437
        rt_FREE(ssGetOffsetTimePtr(rts));

31438
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31439


31440
        /* Destroy model methods 3 */

31441
        {

31442
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31443
          rt_FREE(methods3);

31444
        }

31445


31446
        /* Destroy model methods 2 */

31447
        {

31448
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31449
          rt_FREE(methods2);

31450
        }

31451


31452
        /* Destroy state auxilliary information */

31453
        {

31454
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31455
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31456
          rt_FREE(periodicStatesInfo);

31457
          rt_FREE(statesInfo2);

31458
        }

31459


31460
        {

31461
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31462
          rt_FREE(ptr);

31463
        }

31464


31465
        {

31466
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31467
          rt_FREE(ptr);

31468
        }

31469


31470
        {

31471
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

31472
          rt_FREE(ptr);

31473
        }

31474


31475
        {

31476
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

31477
          rt_FREE(ptr);

31478
        }

31479


31480
        rt_FREE(ssGetPortInfoForInputs(rts));

31481
        rt_FREE(ssGetPortInfoForOutputs(rts));

31482
      }

31483
    }

31484


31485
    /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */

31486
    {

31487
      SimStruct *rts = ssGetSFunction(S, 168);

31488
      if (rts != (NULL) ) {

31489
        rt_FREE(ssGetSampleTimePtr(rts));

31490
        rt_FREE(ssGetOffsetTimePtr(rts));

31491
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31492


31493
        /* Destroy model methods 3 */

31494
        {

31495
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31496
          rt_FREE(methods3);

31497
        }

31498


31499
        /* Destroy model methods 2 */

31500
        {

31501
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31502
          rt_FREE(methods2);

31503
        }

31504


31505
        /* Destroy state auxilliary information */

31506
        {

31507
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31508
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31509
          rt_FREE(periodicStatesInfo);

31510
          rt_FREE(statesInfo2);

31511
        }

31512


31513
        {

31514
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31515
          rt_FREE(ptr);

31516
        }

31517


31518
        {

31519
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31520
          rt_FREE(ptr);

31521
        }

31522


31523
        {

31524
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

31525
          rt_FREE(ptr);

31526
        }

31527


31528
        {

31529
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

31530
          rt_FREE(ptr);

31531
        }

31532


31533
        rt_FREE(ssGetPortInfoForInputs(rts));

31534
        rt_FREE(ssGetPortInfoForOutputs(rts));

31535
      }

31536
    }

31537


31538
    /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */

31539
    {

31540
      SimStruct *rts = ssGetSFunction(S, 169);

31541
      if (rts != (NULL) ) {

31542
        rt_FREE(ssGetSampleTimePtr(rts));

31543
        rt_FREE(ssGetOffsetTimePtr(rts));

31544
        rt_FREE(ssGetSampleTimeTaskIDPtr(rts));

31545


31546
        /* Destroy model methods 3 */

31547
        {

31548
          struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);

31549
          rt_FREE(methods3);

31550
        }

31551


31552
        /* Destroy model methods 2 */

31553
        {

31554
          struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);

31555
          rt_FREE(methods2);

31556
        }

31557


31558
        /* Destroy state auxilliary information */

31559
        {

31560
          struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);

31561
          ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);

31562
          rt_FREE(periodicStatesInfo);

31563
          rt_FREE(statesInfo2);

31564
        }

31565


31566
        {

31567
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);

31568
          rt_FREE(ptr);

31569
        }

31570


31571
        {

31572
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);

31573
          rt_FREE(ptr);

31574
        }

31575


31576
        {

31577
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);

31578
          rt_FREE(ptr);

31579
        }

31580


31581
        {

31582
          void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);

31583
          rt_FREE(ptr);

31584
        }

31585


31586
        rt_FREE(ssGetPortInfoForInputs(rts));

31587
        rt_FREE(ssGetPortInfoForOutputs(rts));

31588
      }

31589
    }

31590


31591
    rt_FREE(ssGetSFunction(S, 0))

31592
      rt_FREE(_ssGetSFunctions(S));

31593
  }

31594


31595
  if (ssGetUserData(S) != (NULL) ) {

31596
    rt_FREE(ssGetLocalBlockIO(S));

31597
    rt_FREE(_ssGetPrevZCSigState(S));

31598
  }

31599


31600
  rt_FREE(ssGetUserData(S));

31601


31602
#endif

31603


31604
}

31605


31606
#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)

31607
#  include "AFCS_MODEL1_mid.h"

31608
#endif

31609


31610
/* Function to initialize sizes. */

31611
static void mdlInitializeSizes(SimStruct *S)

31612
{

31613
  ssSetNumSampleTimes(S, 2);           /* Number of sample times */

31614
  ssSetNumContStates(S, 39);           /* Number of continuous states */

31615
  ssSetNumPeriodicContStates(S, 0);    /* Number of periodic continuous states */

31616
  ssSetNumNonsampledZCs(S, 0);         /* Number of nonsampled ZCs */

31617


31618
  /* Number of output ports */

31619
  if (!ssSetNumOutputPorts(S, 33))

31620
    return;

31621


31622
  /* outport number: 0 */

31623
  if (!ssSetOutputPortVectorDimension(S, 0, 1))

31624
    return;

31625
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31626
    ssSetOutputPortDataType(S, 0, SS_DOUBLE);

31627
  }

31628


31629
  ssSetOutputPortSampleTime(S, 0, 0.0);

31630
  ssSetOutputPortOffsetTime(S, 0, 0.0);

31631
  ssSetOutputPortOptimOpts(S, 0, SS_REUSABLE_AND_LOCAL);

31632


31633
  /* outport number: 1 */

31634
  if (!ssSetOutputPortVectorDimension(S, 1, 1))

31635
    return;

31636
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31637
    ssSetOutputPortDataType(S, 1, SS_DOUBLE);

31638
  }

31639


31640
  ssSetOutputPortSampleTime(S, 1, 0.0);

31641
  ssSetOutputPortOffsetTime(S, 1, 0.0);

31642
  ssSetOutputPortOptimOpts(S, 1, SS_REUSABLE_AND_LOCAL);

31643


31644
  /* outport number: 2 */

31645
  if (!ssSetOutputPortVectorDimension(S, 2, 1))

31646
    return;

31647
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31648
    ssSetOutputPortDataType(S, 2, SS_SINGLE);

31649
  }

31650


31651
  ssSetOutputPortSampleTime(S, 2, 0.0);

31652
  ssSetOutputPortOffsetTime(S, 2, 0.0);

31653
  ssSetOutputPortOptimOpts(S, 2, SS_REUSABLE_AND_LOCAL);

31654


31655
  /* outport number: 3 */

31656
  if (!ssSetOutputPortVectorDimension(S, 3, 1))

31657
    return;

31658
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31659
    ssSetOutputPortDataType(S, 3, SS_SINGLE);

31660
  }

31661


31662
  ssSetOutputPortSampleTime(S, 3, 0.0);

31663
  ssSetOutputPortOffsetTime(S, 3, 0.0);

31664
  ssSetOutputPortOptimOpts(S, 3, SS_REUSABLE_AND_LOCAL);

31665


31666
  /* outport number: 4 */

31667
  if (!ssSetOutputPortVectorDimension(S, 4, 1))

31668
    return;

31669
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31670
    ssSetOutputPortDataType(S, 4, SS_SINGLE);

31671
  }

31672


31673
  ssSetOutputPortSampleTime(S, 4, 0.0);

31674
  ssSetOutputPortOffsetTime(S, 4, 0.0);

31675
  ssSetOutputPortOptimOpts(S, 4, SS_REUSABLE_AND_LOCAL);

31676


31677
  /* outport number: 5 */

31678
  if (!ssSetOutputPortVectorDimension(S, 5, 1))

31679
    return;

31680
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31681
    ssSetOutputPortDataType(S, 5, SS_SINGLE);

31682
  }

31683


31684
  ssSetOutputPortSampleTime(S, 5, 0.0);

31685
  ssSetOutputPortOffsetTime(S, 5, 0.0);

31686
  ssSetOutputPortOptimOpts(S, 5, SS_REUSABLE_AND_LOCAL);

31687


31688
  /* outport number: 6 */

31689
  if (!ssSetOutputPortVectorDimension(S, 6, 1))

31690
    return;

31691
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31692
    ssSetOutputPortDataType(S, 6, SS_SINGLE);

31693
  }

31694


31695
  ssSetOutputPortSampleTime(S, 6, 0.0);

31696
  ssSetOutputPortOffsetTime(S, 6, 0.0);

31697
  ssSetOutputPortOptimOpts(S, 6, SS_REUSABLE_AND_LOCAL);

31698


31699
  /* outport number: 7 */

31700
  if (!ssSetOutputPortVectorDimension(S, 7, 1))

31701
    return;

31702
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31703
    ssSetOutputPortDataType(S, 7, SS_SINGLE);

31704
  }

31705


31706
  ssSetOutputPortSampleTime(S, 7, 0.0);

31707
  ssSetOutputPortOffsetTime(S, 7, 0.0);

31708
  ssSetOutputPortOptimOpts(S, 7, SS_REUSABLE_AND_LOCAL);

31709


31710
  /* outport number: 8 */

31711
  if (!ssSetOutputPortVectorDimension(S, 8, 1))

31712
    return;

31713
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31714
    ssSetOutputPortDataType(S, 8, SS_SINGLE);

31715
  }

31716


31717
  ssSetOutputPortSampleTime(S, 8, 0.0);

31718
  ssSetOutputPortOffsetTime(S, 8, 0.0);

31719
  ssSetOutputPortOptimOpts(S, 8, SS_REUSABLE_AND_LOCAL);

31720


31721
  /* outport number: 9 */

31722
  if (!ssSetOutputPortVectorDimension(S, 9, 1))

31723
    return;

31724
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31725
    ssSetOutputPortDataType(S, 9, SS_SINGLE);

31726
  }

31727


31728
  ssSetOutputPortSampleTime(S, 9, 0.0);

31729
  ssSetOutputPortOffsetTime(S, 9, 0.0);

31730
  ssSetOutputPortOptimOpts(S, 9, SS_REUSABLE_AND_LOCAL);

31731


31732
  /* outport number: 10 */

31733
  if (!ssSetOutputPortVectorDimension(S, 10, 1))

31734
    return;

31735
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31736
    ssSetOutputPortDataType(S, 10, SS_SINGLE);

31737
  }

31738


31739
  ssSetOutputPortSampleTime(S, 10, 0.0);

31740
  ssSetOutputPortOffsetTime(S, 10, 0.0);

31741
  ssSetOutputPortOptimOpts(S, 10, SS_REUSABLE_AND_LOCAL);

31742


31743
  /* outport number: 11 */

31744
  if (!ssSetOutputPortVectorDimension(S, 11, 1))

31745
    return;

31746
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31747
    ssSetOutputPortDataType(S, 11, SS_SINGLE);

31748
  }

31749


31750
  ssSetOutputPortSampleTime(S, 11, 0.0);

31751
  ssSetOutputPortOffsetTime(S, 11, 0.0);

31752
  ssSetOutputPortOptimOpts(S, 11, SS_REUSABLE_AND_LOCAL);

31753


31754
  /* outport number: 12 */

31755
  if (!ssSetOutputPortVectorDimension(S, 12, 1))

31756
    return;

31757
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31758
    ssSetOutputPortDataType(S, 12, SS_SINGLE);

31759
  }

31760


31761
  ssSetOutputPortSampleTime(S, 12, 0.0);

31762
  ssSetOutputPortOffsetTime(S, 12, 0.0);

31763
  ssSetOutputPortOptimOpts(S, 12, SS_REUSABLE_AND_LOCAL);

31764


31765
  /* outport number: 13 */

31766
  if (!ssSetOutputPortVectorDimension(S, 13, 1))

31767
    return;

31768
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31769
    ssSetOutputPortDataType(S, 13, SS_SINGLE);

31770
  }

31771


31772
  ssSetOutputPortSampleTime(S, 13, 0.0);

31773
  ssSetOutputPortOffsetTime(S, 13, 0.0);

31774
  ssSetOutputPortOptimOpts(S, 13, SS_REUSABLE_AND_LOCAL);

31775


31776
  /* outport number: 14 */

31777
  if (!ssSetOutputPortVectorDimension(S, 14, 1))

31778
    return;

31779
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31780
    ssSetOutputPortDataType(S, 14, SS_SINGLE);

31781
  }

31782


31783
  ssSetOutputPortSampleTime(S, 14, 0.0);

31784
  ssSetOutputPortOffsetTime(S, 14, 0.0);

31785
  ssSetOutputPortOptimOpts(S, 14, SS_REUSABLE_AND_LOCAL);

31786


31787
  /* outport number: 15 */

31788
  if (!ssSetOutputPortVectorDimension(S, 15, 1))

31789
    return;

31790
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31791
    ssSetOutputPortDataType(S, 15, SS_SINGLE);

31792
  }

31793


31794
  ssSetOutputPortSampleTime(S, 15, 0.0);

31795
  ssSetOutputPortOffsetTime(S, 15, 0.0);

31796
  ssSetOutputPortOptimOpts(S, 15, SS_REUSABLE_AND_LOCAL);

31797


31798
  /* outport number: 16 */

31799
  if (!ssSetOutputPortVectorDimension(S, 16, 1))

31800
    return;

31801
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31802
    ssSetOutputPortDataType(S, 16, SS_SINGLE);

31803
  }

31804


31805
  ssSetOutputPortSampleTime(S, 16, 0.0);

31806
  ssSetOutputPortOffsetTime(S, 16, 0.0);

31807
  ssSetOutputPortOptimOpts(S, 16, SS_REUSABLE_AND_LOCAL);

31808


31809
  /* outport number: 17 */

31810
  if (!ssSetOutputPortMatrixDimensions(S, 17, 1, 1))

31811
    return;

31812
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31813
    ssSetOutputPortDataType(S, 17, SS_SINGLE);

31814
  }

31815


31816
  ssSetOutputPortSampleTime(S, 17, 0.0);

31817
  ssSetOutputPortOffsetTime(S, 17, 0.0);

31818
  ssSetOutputPortOptimOpts(S, 17, SS_REUSABLE_AND_LOCAL);

31819


31820
  /* outport number: 18 */

31821
  if (!ssSetOutputPortVectorDimension(S, 18, 1))

31822
    return;

31823
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31824
    ssSetOutputPortDataType(S, 18, SS_SINGLE);

31825
  }

31826


31827
  ssSetOutputPortSampleTime(S, 18, 0.0);

31828
  ssSetOutputPortOffsetTime(S, 18, 0.0);

31829
  ssSetOutputPortOptimOpts(S, 18, SS_REUSABLE_AND_LOCAL);

31830


31831
  /* outport number: 19 */

31832
  if (!ssSetOutputPortVectorDimension(S, 19, 1))

31833
    return;

31834
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31835
    ssSetOutputPortDataType(S, 19, SS_SINGLE);

31836
  }

31837


31838
  ssSetOutputPortSampleTime(S, 19, 0.0);

31839
  ssSetOutputPortOffsetTime(S, 19, 0.0);

31840
  ssSetOutputPortOptimOpts(S, 19, SS_REUSABLE_AND_LOCAL);

31841


31842
  /* outport number: 20 */

31843
  if (!ssSetOutputPortVectorDimension(S, 20, 1))

31844
    return;

31845
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31846
    ssSetOutputPortDataType(S, 20, SS_DOUBLE);

31847
  }

31848


31849
  ssSetOutputPortSampleTime(S, 20, rtInf);

31850
  ssSetOutputPortOffsetTime(S, 20, 0.0);

31851
  ssSetOutputPortOptimOpts(S, 20, SS_NOT_REUSABLE_AND_LOCAL);

31852


31853
  /* outport number: 21 */

31854
  if (!ssSetOutputPortVectorDimension(S, 21, 1))

31855
    return;

31856
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31857
    ssSetOutputPortDataType(S, 21, SS_DOUBLE);

31858
  }

31859


31860
  ssSetOutputPortSampleTime(S, 21, rtInf);

31861
  ssSetOutputPortOffsetTime(S, 21, 0.0);

31862
  ssSetOutputPortOptimOpts(S, 21, SS_NOT_REUSABLE_AND_LOCAL);

31863


31864
  /* outport number: 22 */

31865
  if (!ssSetOutputPortVectorDimension(S, 22, 1))

31866
    return;

31867
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31868
    ssSetOutputPortDataType(S, 22, SS_DOUBLE);

31869
  }

31870


31871
  ssSetOutputPortSampleTime(S, 22, rtInf);

31872
  ssSetOutputPortOffsetTime(S, 22, 0.0);

31873
  ssSetOutputPortOptimOpts(S, 22, SS_NOT_REUSABLE_AND_LOCAL);

31874


31875
  /* outport number: 23 */

31876
  if (!ssSetOutputPortVectorDimension(S, 23, 1))

31877
    return;

31878
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31879
    ssSetOutputPortDataType(S, 23, SS_DOUBLE);

31880
  }

31881


31882
  ssSetOutputPortSampleTime(S, 23, rtInf);

31883
  ssSetOutputPortOffsetTime(S, 23, 0.0);

31884
  ssSetOutputPortOptimOpts(S, 23, SS_NOT_REUSABLE_AND_LOCAL);

31885


31886
  /* outport number: 24 */

31887
  if (!ssSetOutputPortVectorDimension(S, 24, 1))

31888
    return;

31889
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31890
    ssSetOutputPortDataType(S, 24, SS_SINGLE);

31891
  }

31892


31893
  ssSetOutputPortSampleTime(S, 24, 0.0);

31894
  ssSetOutputPortOffsetTime(S, 24, 0.0);

31895
  ssSetOutputPortOptimOpts(S, 24, SS_REUSABLE_AND_LOCAL);

31896


31897
  /* outport number: 25 */

31898
  if (!ssSetOutputPortVectorDimension(S, 25, 1))

31899
    return;

31900
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31901
    ssSetOutputPortDataType(S, 25, SS_SINGLE);

31902
  }

31903


31904
  ssSetOutputPortSampleTime(S, 25, 0.0);

31905
  ssSetOutputPortOffsetTime(S, 25, 0.0);

31906
  ssSetOutputPortOptimOpts(S, 25, SS_REUSABLE_AND_LOCAL);

31907


31908
  /* outport number: 26 */

31909
  if (!ssSetOutputPortVectorDimension(S, 26, 1))

31910
    return;

31911
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31912
    ssSetOutputPortDataType(S, 26, SS_SINGLE);

31913
  }

31914


31915
  ssSetOutputPortSampleTime(S, 26, 0.0);

31916
  ssSetOutputPortOffsetTime(S, 26, 0.0);

31917
  ssSetOutputPortOptimOpts(S, 26, SS_REUSABLE_AND_LOCAL);

31918


31919
  /* outport number: 27 */

31920
  if (!ssSetOutputPortVectorDimension(S, 27, 1))

31921
    return;

31922
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31923
    ssSetOutputPortDataType(S, 27, SS_SINGLE);

31924
  }

31925


31926
  ssSetOutputPortSampleTime(S, 27, 0.0);

31927
  ssSetOutputPortOffsetTime(S, 27, 0.0);

31928
  ssSetOutputPortOptimOpts(S, 27, SS_REUSABLE_AND_LOCAL);

31929


31930
  /* outport number: 28 */

31931
  if (!ssSetOutputPortVectorDimension(S, 28, 1))

31932
    return;

31933
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31934
    ssSetOutputPortDataType(S, 28, SS_SINGLE);

31935
  }

31936


31937
  ssSetOutputPortSampleTime(S, 28, 0.0);

31938
  ssSetOutputPortOffsetTime(S, 28, 0.0);

31939
  ssSetOutputPortOptimOpts(S, 28, SS_REUSABLE_AND_LOCAL);

31940


31941
  /* outport number: 29 */

31942
  if (!ssSetOutputPortVectorDimension(S, 29, 1))

31943
    return;

31944
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31945
    ssSetOutputPortDataType(S, 29, SS_SINGLE);

31946
  }

31947


31948
  ssSetOutputPortSampleTime(S, 29, 0.0);

31949
  ssSetOutputPortOffsetTime(S, 29, 0.0);

31950
  ssSetOutputPortOptimOpts(S, 29, SS_REUSABLE_AND_LOCAL);

31951


31952
  /* outport number: 30 */

31953
  if (!ssSetOutputPortVectorDimension(S, 30, 1))

31954
    return;

31955
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31956
    ssSetOutputPortDataType(S, 30, SS_SINGLE);

31957
  }

31958


31959
  ssSetOutputPortSampleTime(S, 30, 0.0);

31960
  ssSetOutputPortOffsetTime(S, 30, 0.0);

31961
  ssSetOutputPortOptimOpts(S, 30, SS_REUSABLE_AND_LOCAL);

31962


31963
  /* outport number: 31 */

31964
  if (!ssSetOutputPortVectorDimension(S, 31, 1))

31965
    return;

31966
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31967
    ssSetOutputPortDataType(S, 31, SS_SINGLE);

31968
  }

31969


31970
  ssSetOutputPortSampleTime(S, 31, 0.0);

31971
  ssSetOutputPortOffsetTime(S, 31, 0.0);

31972
  ssSetOutputPortOptimOpts(S, 31, SS_REUSABLE_AND_LOCAL);

31973


31974
  /* outport number: 32 */

31975
  if (!ssSetOutputPortVectorDimension(S, 32, 1))

31976
    return;

31977
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31978
    ssSetOutputPortDataType(S, 32, SS_INT8);

31979
  }

31980


31981
  ssSetOutputPortSampleTime(S, 32, 0.005);

31982
  ssSetOutputPortOffsetTime(S, 32, 0.0);

31983
  ssSetOutputPortOptimOpts(S, 32, SS_REUSABLE_AND_LOCAL);

31984


31985
  /* Number of input ports */

31986
  if (!ssSetNumInputPorts(S, 12))

31987
    return;

31988


31989
  /* inport number: 0 */

31990
  {

31991
    if (!ssSetInputPortVectorDimension(S, 0, 1))

31992
      return;

31993
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

31994
      ssSetInputPortDataType(S, 0, SS_INT8);

31995
    }

31996


31997
    ssSetInputPortDirectFeedThrough(S, 0, 1);

31998
    ssSetInputPortSampleTime(S, 0, 0.0);

31999
    ssSetInputPortOffsetTime(S, 0, 0.0);

32000
    ssSetInputPortOverWritable(S, 0, 0);

32001
    ssSetInputPortOptimOpts(S, 0, SS_NOT_REUSABLE_AND_GLOBAL);

32002
  }

32003


32004
  /* inport number: 1 */

32005
  {

32006
    if (!ssSetInputPortVectorDimension(S, 1, 1))

32007
      return;

32008
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32009
      ssSetInputPortDataType(S, 1, SS_INT8);

32010
    }

32011


32012
    ssSetInputPortDirectFeedThrough(S, 1, 1);

32013
    ssSetInputPortSampleTime(S, 1, 0.0);

32014
    ssSetInputPortOffsetTime(S, 1, 0.0);

32015
    ssSetInputPortOverWritable(S, 1, 0);

32016
    ssSetInputPortOptimOpts(S, 1, SS_NOT_REUSABLE_AND_GLOBAL);

32017
  }

32018


32019
  /* inport number: 2 */

32020
  {

32021
    if (!ssSetInputPortVectorDimension(S, 2, 1))

32022
      return;

32023
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32024
      ssSetInputPortDataType(S, 2, SS_SINGLE);

32025
    }

32026


32027
    ssSetInputPortDirectFeedThrough(S, 2, 1);

32028
    ssSetInputPortSampleTime(S, 2, 0.0);

32029
    ssSetInputPortOffsetTime(S, 2, 0.0);

32030
    ssSetInputPortOverWritable(S, 2, 0);

32031
    ssSetInputPortOptimOpts(S, 2, SS_NOT_REUSABLE_AND_GLOBAL);

32032
  }

32033


32034
  /* inport number: 3 */

32035
  {

32036
    if (!ssSetInputPortVectorDimension(S, 3, 1))

32037
      return;

32038
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32039
      ssSetInputPortDataType(S, 3, SS_SINGLE);

32040
    }

32041


32042
    ssSetInputPortDirectFeedThrough(S, 3, 1);

32043
    ssSetInputPortSampleTime(S, 3, 0.0);

32044
    ssSetInputPortOffsetTime(S, 3, 0.0);

32045
    ssSetInputPortOverWritable(S, 3, 0);

32046
    ssSetInputPortOptimOpts(S, 3, SS_NOT_REUSABLE_AND_GLOBAL);

32047
  }

32048


32049
  /* inport number: 4 */

32050
  {

32051
    if (!ssSetInputPortVectorDimension(S, 4, 1))

32052
      return;

32053
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32054
      ssSetInputPortDataType(S, 4, SS_SINGLE);

32055
    }

32056


32057
    ssSetInputPortDirectFeedThrough(S, 4, 1);

32058
    ssSetInputPortSampleTime(S, 4, 0.0);

32059
    ssSetInputPortOffsetTime(S, 4, 0.0);

32060
    ssSetInputPortOverWritable(S, 4, 0);

32061
    ssSetInputPortOptimOpts(S, 4, SS_NOT_REUSABLE_AND_GLOBAL);

32062
  }

32063


32064
  /* inport number: 5 */

32065
  {

32066
    if (!ssSetInputPortVectorDimension(S, 5, 1))

32067
      return;

32068
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32069
      ssSetInputPortDataType(S, 5, SS_DOUBLE);

32070
    }

32071


32072
    ssSetInputPortDirectFeedThrough(S, 5, 1);

32073
    ssSetInputPortSampleTime(S, 5, 0.0);

32074
    ssSetInputPortOffsetTime(S, 5, 0.0);

32075
    ssSetInputPortOverWritable(S, 5, 0);

32076
    ssSetInputPortOptimOpts(S, 5, SS_NOT_REUSABLE_AND_GLOBAL);

32077
  }

32078


32079
  /* inport number: 6 */

32080
  {

32081
    if (!ssSetInputPortVectorDimension(S, 6, 1))

32082
      return;

32083
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32084
      ssSetInputPortDataType(S, 6, SS_DOUBLE);

32085
    }

32086


32087
    ssSetInputPortDirectFeedThrough(S, 6, 1);

32088
    ssSetInputPortSampleTime(S, 6, 0.0);

32089
    ssSetInputPortOffsetTime(S, 6, 0.0);

32090
    ssSetInputPortOverWritable(S, 6, 0);

32091
    ssSetInputPortOptimOpts(S, 6, SS_NOT_REUSABLE_AND_GLOBAL);

32092
  }

32093


32094
  /* inport number: 7 */

32095
  {

32096
    if (!ssSetInputPortVectorDimension(S, 7, 1))

32097
      return;

32098
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32099
      ssSetInputPortDataType(S, 7, SS_DOUBLE);

32100
    }

32101


32102
    ssSetInputPortDirectFeedThrough(S, 7, 1);

32103
    ssSetInputPortSampleTime(S, 7, 0.0);

32104
    ssSetInputPortOffsetTime(S, 7, 0.0);

32105
    ssSetInputPortOverWritable(S, 7, 0);

32106
    ssSetInputPortOptimOpts(S, 7, SS_NOT_REUSABLE_AND_GLOBAL);

32107
  }

32108


32109
  /* inport number: 8 */

32110
  {

32111
    if (!ssSetInputPortMatrixDimensions(S, 8, 3, 30))

32112
      return;

32113
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32114
      ssSetInputPortDataType(S, 8, SS_DOUBLE);

32115
    }

32116


32117
    ssSetInputPortDirectFeedThrough(S, 8, 1);

32118
    ssSetInputPortSampleTime(S, 8, 0.0);

32119
    ssSetInputPortOffsetTime(S, 8, 0.0);

32120
    ssSetInputPortOverWritable(S, 8, 0);

32121
    ssSetInputPortOptimOpts(S, 8, SS_NOT_REUSABLE_AND_GLOBAL);

32122
  }

32123


32124
  /* inport number: 9 */

32125
  {

32126
    if (!ssSetInputPortVectorDimension(S, 9, 1))

32127
      return;

32128
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32129
      ssSetInputPortDataType(S, 9, SS_DOUBLE);

32130
    }

32131


32132
    ssSetInputPortDirectFeedThrough(S, 9, 1);

32133
    ssSetInputPortSampleTime(S, 9, 0.0);

32134
    ssSetInputPortOffsetTime(S, 9, 0.0);

32135
    ssSetInputPortOverWritable(S, 9, 0);

32136
    ssSetInputPortOptimOpts(S, 9, SS_NOT_REUSABLE_AND_GLOBAL);

32137
  }

32138


32139
  /* inport number: 10 */

32140
  {

32141
    if (!ssSetInputPortVectorDimension(S, 10, 1))

32142
      return;

32143
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32144
      ssSetInputPortDataType(S, 10, SS_DOUBLE);

32145
    }

32146


32147
    ssSetInputPortDirectFeedThrough(S, 10, 1);

32148
    ssSetInputPortSampleTime(S, 10, 0.0);

32149
    ssSetInputPortOffsetTime(S, 10, 0.0);

32150
    ssSetInputPortOverWritable(S, 10, 0);

32151
    ssSetInputPortOptimOpts(S, 10, SS_NOT_REUSABLE_AND_GLOBAL);

32152
  }

32153


32154
  /* inport number: 11 */

32155
  {

32156
    if (!ssSetInputPortVectorDimension(S, 11, 1))

32157
      return;

32158
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32159
      ssSetInputPortDataType(S, 11, SS_DOUBLE);

32160
    }

32161


32162
    ssSetInputPortDirectFeedThrough(S, 11, 1);

32163
    ssSetInputPortSampleTime(S, 11, 0.0);

32164
    ssSetInputPortOffsetTime(S, 11, 0.0);

32165
    ssSetInputPortOverWritable(S, 11, 0);

32166
    ssSetInputPortOptimOpts(S, 11, SS_NOT_REUSABLE_AND_GLOBAL);

32167
  }

32168


32169
  ssSetRTWGeneratedSFcn(S, 1);         /* Generated S-function */

32170


32171
  /* DWork */

32172
  if (!ssSetNumDWork(S, 641)) {

32173
    return;

32174
  }

32175


32176
  /* '<S619>/Unit Delay1': DSTATE */

32177
  ssSetDWorkName(S, 0, "DWORK0");

32178
  ssSetDWorkWidth(S, 0, 1);

32179
  ssSetDWorkUsedAsDState(S, 0, 1);

32180


32181
  /* '<S423>/Output': DSTATE */

32182
  ssSetDWorkName(S, 1, "DWORK1");

32183
  ssSetDWorkWidth(S, 1, 1);

32184
  ssFxpSetU32BitRegionCompliant(S, 1);

32185
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {

32186


32187
#if defined (MATLAB_MEX_FILE)

32188


32189
    {

32190
      DTypeId dataTypeIdReg =

32191
        ssRegisterDataTypeFxpFSlopeFixExpBias(

32192
        S,

32193
        0,

32194
        64,

32195
        1.0,

32196
        0,

32197
        0.0,

32198
        0                              /* false means do NOT obey data type override setting for this subsystem */

32199
        );

32200
      ssSetDWorkDataType(S, 1, dataTypeIdReg );

32201
    }

32202


32203
#endif

32204


32205
  }

32206


32207
  ssSetDWorkUsedAsDState(S, 1, 1);

32208


32209
  /* '<S465>/Rate Limiter': PrevY */

32210
  ssSetDWorkName(S, 2, "DWORK2");

32211
  ssSetDWorkWidth(S, 2, 3);

32212


32213
  /* '<S465>/Rate Limiter': LastMajorTime */

32214
  ssSetDWorkName(S, 3, "DWORK3");

32215
  ssSetDWorkWidth(S, 3, 1);

32216


32217
  /* '<S446>/Backlash': DWORK1 */

32218
  ssSetDWorkName(S, 4, "DWORK4");

32219
  ssSetDWorkWidth(S, 4, 1);

32220


32221
  /* '<S954>/WGS84 Gravity S-Function': h */

32222
  ssSetDWorkName(S, 5, "DWORK5");

32223
  ssSetDWorkWidth(S, 5, 1);

32224


32225
  /* '<S954>/WGS84 Gravity S-Function': phi */

32226
  ssSetDWorkName(S, 6, "DWORK6");

32227
  ssSetDWorkWidth(S, 6, 1);

32228


32229
  /* '<S954>/WGS84 Gravity S-Function': lambda */

32230
  ssSetDWorkName(S, 7, "DWORK7");

32231
  ssSetDWorkWidth(S, 7, 1);

32232


32233
  /* '<S954>/WGS84 Gravity S-Function': gamma_h */

32234
  ssSetDWorkName(S, 8, "DWORK8");

32235
  ssSetDWorkWidth(S, 8, 1);

32236


32237
  /* '<S954>/WGS84 Gravity S-Function': gamma_phi */

32238
  ssSetDWorkName(S, 9, "DWORK9");

32239
  ssSetDWorkWidth(S, 9, 1);

32240


32241
  /* '<S777>/Product2': DWORK4 */

32242
  ssSetDWorkName(S, 10, "DWORK10");

32243
  ssSetDWorkWidth(S, 10, 9);

32244


32245
  /* '<S793>/Memory': PreviousInput */

32246
  ssSetDWorkName(S, 11, "DWORK11");

32247
  ssSetDWorkWidth(S, 11, 1);

32248


32249
  /* '<S335>/Data Store Memory10': dsmMem */

32250
  ssSetDWorkName(S, 12, "DWORK12");

32251
  ssSetDWorkWidth(S, 12, 32);

32252


32253
  /* '<S335>/Data Store Memory5': dsmMem */

32254
  ssSetDWorkName(S, 13, "DWORK13");

32255
  ssSetDWorkWidth(S, 13, 32);

32256


32257
  /* '<S335>/Data Store Memory6': dsmMem */

32258
  ssSetDWorkName(S, 14, "DWORK14");

32259
  ssSetDWorkWidth(S, 14, 32);

32260


32261
  /* '<S335>/Data Store Memory7': dsmMem */

32262
  ssSetDWorkName(S, 15, "DWORK15");

32263
  ssSetDWorkWidth(S, 15, 32);

32264


32265
  /* '<S335>/Data Store Memory8': dsmMem */

32266
  ssSetDWorkName(S, 16, "DWORK16");

32267
  ssSetDWorkWidth(S, 16, 32);

32268


32269
  /* '<S335>/Data Store Memory9': dsmMem */

32270
  ssSetDWorkName(S, 17, "DWORK17");

32271
  ssSetDWorkWidth(S, 17, 32);

32272


32273
  /* '<S6>/Nz': PWORK */

32274
  ssSetDWorkName(S, 18, "DWORK18");

32275
  ssSetDWorkWidth(S, 18, 1);

32276
  ssSetDWorkDataType(S, 18, SS_POINTER);

32277


32278
  /* '<S6>/PhiL1': PWORK */

32279
  ssSetDWorkName(S, 19, "DWORK19");

32280
  ssSetDWorkWidth(S, 19, 2);

32281
  ssSetDWorkDataType(S, 19, SS_POINTER);

32282


32283
  /* '<S6>/PhiL2': PWORK */

32284
  ssSetDWorkName(S, 20, "DWORK20");

32285
  ssSetDWorkWidth(S, 20, 2);

32286
  ssSetDWorkDataType(S, 20, SS_POINTER);

32287


32288
  /* '<S6>/PhiL3': PWORK */

32289
  ssSetDWorkName(S, 21, "DWORK21");

32290
  ssSetDWorkWidth(S, 21, 2);

32291
  ssSetDWorkDataType(S, 21, SS_POINTER);

32292


32293
  /* '<S6>/PhiL4': PWORK */

32294
  ssSetDWorkName(S, 22, "DWORK22");

32295
  ssSetDWorkWidth(S, 22, 2);

32296
  ssSetDWorkDataType(S, 22, SS_POINTER);

32297


32298
  /* '<S6>/Scope': PWORK */

32299
  ssSetDWorkName(S, 23, "DWORK23");

32300
  ssSetDWorkWidth(S, 23, 1);

32301
  ssSetDWorkDataType(S, 23, SS_POINTER);

32302


32303
  /* '<S6>/Scope1': PWORK */

32304
  ssSetDWorkName(S, 24, "DWORK24");

32305
  ssSetDWorkWidth(S, 24, 1);

32306
  ssSetDWorkDataType(S, 24, SS_POINTER);

32307


32308
  /* '<S6>/Scope10': PWORK */

32309
  ssSetDWorkName(S, 25, "DWORK25");

32310
  ssSetDWorkWidth(S, 25, 1);

32311
  ssSetDWorkDataType(S, 25, SS_POINTER);

32312


32313
  /* '<S6>/Scope11': PWORK */

32314
  ssSetDWorkName(S, 26, "DWORK26");

32315
  ssSetDWorkWidth(S, 26, 1);

32316
  ssSetDWorkDataType(S, 26, SS_POINTER);

32317


32318
  /* '<S6>/Scope12': PWORK */

32319
  ssSetDWorkName(S, 27, "DWORK27");

32320
  ssSetDWorkWidth(S, 27, 1);

32321
  ssSetDWorkDataType(S, 27, SS_POINTER);

32322


32323
  /* '<S6>/Scope14': PWORK */

32324
  ssSetDWorkName(S, 28, "DWORK28");

32325
  ssSetDWorkWidth(S, 28, 1);

32326
  ssSetDWorkDataType(S, 28, SS_POINTER);

32327


32328
  /* '<S6>/Scope15': PWORK */

32329
  ssSetDWorkName(S, 29, "DWORK29");

32330
  ssSetDWorkWidth(S, 29, 1);

32331
  ssSetDWorkDataType(S, 29, SS_POINTER);

32332


32333
  /* '<S6>/Scope16': PWORK */

32334
  ssSetDWorkName(S, 30, "DWORK30");

32335
  ssSetDWorkWidth(S, 30, 1);

32336
  ssSetDWorkDataType(S, 30, SS_POINTER);

32337


32338
  /* '<S6>/Scope17': PWORK */

32339
  ssSetDWorkName(S, 31, "DWORK31");

32340
  ssSetDWorkWidth(S, 31, 1);

32341
  ssSetDWorkDataType(S, 31, SS_POINTER);

32342


32343
  /* '<S6>/Scope18': PWORK */

32344
  ssSetDWorkName(S, 32, "DWORK32");

32345
  ssSetDWorkWidth(S, 32, 1);

32346
  ssSetDWorkDataType(S, 32, SS_POINTER);

32347


32348
  /* '<S6>/Scope19': PWORK */

32349
  ssSetDWorkName(S, 33, "DWORK33");

32350
  ssSetDWorkWidth(S, 33, 1);

32351
  ssSetDWorkDataType(S, 33, SS_POINTER);

32352


32353
  /* '<S6>/Scope2': PWORK */

32354
  ssSetDWorkName(S, 34, "DWORK34");

32355
  ssSetDWorkWidth(S, 34, 1);

32356
  ssSetDWorkDataType(S, 34, SS_POINTER);

32357


32358
  /* '<S6>/Scope20': PWORK */

32359
  ssSetDWorkName(S, 35, "DWORK35");

32360
  ssSetDWorkWidth(S, 35, 1);

32361
  ssSetDWorkDataType(S, 35, SS_POINTER);

32362


32363
  /* '<S6>/Scope21': PWORK */

32364
  ssSetDWorkName(S, 36, "DWORK36");

32365
  ssSetDWorkWidth(S, 36, 1);

32366
  ssSetDWorkDataType(S, 36, SS_POINTER);

32367


32368
  /* '<S6>/Scope22': PWORK */

32369
  ssSetDWorkName(S, 37, "DWORK37");

32370
  ssSetDWorkWidth(S, 37, 1);

32371
  ssSetDWorkDataType(S, 37, SS_POINTER);

32372


32373
  /* '<S6>/Scope23': PWORK */

32374
  ssSetDWorkName(S, 38, "DWORK38");

32375
  ssSetDWorkWidth(S, 38, 1);

32376
  ssSetDWorkDataType(S, 38, SS_POINTER);

32377


32378
  /* '<S6>/Scope24': PWORK */

32379
  ssSetDWorkName(S, 39, "DWORK39");

32380
  ssSetDWorkWidth(S, 39, 1);

32381
  ssSetDWorkDataType(S, 39, SS_POINTER);

32382


32383
  /* '<S6>/Scope25': PWORK */

32384
  ssSetDWorkName(S, 40, "DWORK40");

32385
  ssSetDWorkWidth(S, 40, 1);

32386
  ssSetDWorkDataType(S, 40, SS_POINTER);

32387


32388
  /* '<S6>/Scope26': PWORK */

32389
  ssSetDWorkName(S, 41, "DWORK41");

32390
  ssSetDWorkWidth(S, 41, 1);

32391
  ssSetDWorkDataType(S, 41, SS_POINTER);

32392


32393
  /* '<S6>/Scope27': PWORK */

32394
  ssSetDWorkName(S, 42, "DWORK42");

32395
  ssSetDWorkWidth(S, 42, 1);

32396
  ssSetDWorkDataType(S, 42, SS_POINTER);

32397


32398
  /* '<S6>/Scope28': PWORK */

32399
  ssSetDWorkName(S, 43, "DWORK43");

32400
  ssSetDWorkWidth(S, 43, 1);

32401
  ssSetDWorkDataType(S, 43, SS_POINTER);

32402


32403
  /* '<S6>/Scope29': PWORK */

32404
  ssSetDWorkName(S, 44, "DWORK44");

32405
  ssSetDWorkWidth(S, 44, 1);

32406
  ssSetDWorkDataType(S, 44, SS_POINTER);

32407


32408
  /* '<S6>/Scope3': PWORK */

32409
  ssSetDWorkName(S, 45, "DWORK45");

32410
  ssSetDWorkWidth(S, 45, 1);

32411
  ssSetDWorkDataType(S, 45, SS_POINTER);

32412


32413
  /* '<S6>/Scope30': PWORK */

32414
  ssSetDWorkName(S, 46, "DWORK46");

32415
  ssSetDWorkWidth(S, 46, 1);

32416
  ssSetDWorkDataType(S, 46, SS_POINTER);

32417


32418
  /* '<S6>/Scope31': PWORK */

32419
  ssSetDWorkName(S, 47, "DWORK47");

32420
  ssSetDWorkWidth(S, 47, 1);

32421
  ssSetDWorkDataType(S, 47, SS_POINTER);

32422


32423
  /* '<S6>/Scope32': PWORK */

32424
  ssSetDWorkName(S, 48, "DWORK48");

32425
  ssSetDWorkWidth(S, 48, 1);

32426
  ssSetDWorkDataType(S, 48, SS_POINTER);

32427


32428
  /* '<S6>/Scope33': PWORK */

32429
  ssSetDWorkName(S, 49, "DWORK49");

32430
  ssSetDWorkWidth(S, 49, 1);

32431
  ssSetDWorkDataType(S, 49, SS_POINTER);

32432


32433
  /* '<S6>/Scope34': PWORK */

32434
  ssSetDWorkName(S, 50, "DWORK50");

32435
  ssSetDWorkWidth(S, 50, 1);

32436
  ssSetDWorkDataType(S, 50, SS_POINTER);

32437


32438
  /* '<S6>/Scope35': PWORK */

32439
  ssSetDWorkName(S, 51, "DWORK51");

32440
  ssSetDWorkWidth(S, 51, 1);

32441
  ssSetDWorkDataType(S, 51, SS_POINTER);

32442


32443
  /* '<S6>/Scope36': PWORK */

32444
  ssSetDWorkName(S, 52, "DWORK52");

32445
  ssSetDWorkWidth(S, 52, 1);

32446
  ssSetDWorkDataType(S, 52, SS_POINTER);

32447


32448
  /* '<S6>/Scope37': PWORK */

32449
  ssSetDWorkName(S, 53, "DWORK53");

32450
  ssSetDWorkWidth(S, 53, 1);

32451
  ssSetDWorkDataType(S, 53, SS_POINTER);

32452


32453
  /* '<S6>/Scope38': PWORK */

32454
  ssSetDWorkName(S, 54, "DWORK54");

32455
  ssSetDWorkWidth(S, 54, 1);

32456
  ssSetDWorkDataType(S, 54, SS_POINTER);

32457


32458
  /* '<S6>/Scope39': PWORK */

32459
  ssSetDWorkName(S, 55, "DWORK55");

32460
  ssSetDWorkWidth(S, 55, 1);

32461
  ssSetDWorkDataType(S, 55, SS_POINTER);

32462


32463
  /* '<S6>/Scope4': PWORK */

32464
  ssSetDWorkName(S, 56, "DWORK56");

32465
  ssSetDWorkWidth(S, 56, 1);

32466
  ssSetDWorkDataType(S, 56, SS_POINTER);

32467


32468
  /* '<S6>/Scope40': PWORK */

32469
  ssSetDWorkName(S, 57, "DWORK57");

32470
  ssSetDWorkWidth(S, 57, 1);

32471
  ssSetDWorkDataType(S, 57, SS_POINTER);

32472


32473
  /* '<S6>/Scope41': PWORK */

32474
  ssSetDWorkName(S, 58, "DWORK58");

32475
  ssSetDWorkWidth(S, 58, 1);

32476
  ssSetDWorkDataType(S, 58, SS_POINTER);

32477


32478
  /* '<S6>/Scope42': PWORK */

32479
  ssSetDWorkName(S, 59, "DWORK59");

32480
  ssSetDWorkWidth(S, 59, 1);

32481
  ssSetDWorkDataType(S, 59, SS_POINTER);

32482


32483
  /* '<S6>/Scope43': PWORK */

32484
  ssSetDWorkName(S, 60, "DWORK60");

32485
  ssSetDWorkWidth(S, 60, 1);

32486
  ssSetDWorkDataType(S, 60, SS_POINTER);

32487


32488
  /* '<S6>/Scope44': PWORK */

32489
  ssSetDWorkName(S, 61, "DWORK61");

32490
  ssSetDWorkWidth(S, 61, 1);

32491
  ssSetDWorkDataType(S, 61, SS_POINTER);

32492


32493
  /* '<S6>/Scope45': PWORK */

32494
  ssSetDWorkName(S, 62, "DWORK62");

32495
  ssSetDWorkWidth(S, 62, 1);

32496
  ssSetDWorkDataType(S, 62, SS_POINTER);

32497


32498
  /* '<S6>/Scope46': PWORK */

32499
  ssSetDWorkName(S, 63, "DWORK63");

32500
  ssSetDWorkWidth(S, 63, 1);

32501
  ssSetDWorkDataType(S, 63, SS_POINTER);

32502


32503
  /* '<S6>/Scope47': PWORK */

32504
  ssSetDWorkName(S, 64, "DWORK64");

32505
  ssSetDWorkWidth(S, 64, 1);

32506
  ssSetDWorkDataType(S, 64, SS_POINTER);

32507


32508
  /* '<S6>/Scope48': PWORK */

32509
  ssSetDWorkName(S, 65, "DWORK65");

32510
  ssSetDWorkWidth(S, 65, 1);

32511
  ssSetDWorkDataType(S, 65, SS_POINTER);

32512


32513
  /* '<S6>/Scope49': PWORK */

32514
  ssSetDWorkName(S, 66, "DWORK66");

32515
  ssSetDWorkWidth(S, 66, 1);

32516
  ssSetDWorkDataType(S, 66, SS_POINTER);

32517


32518
  /* '<S6>/Scope5': PWORK */

32519
  ssSetDWorkName(S, 67, "DWORK67");

32520
  ssSetDWorkWidth(S, 67, 1);

32521
  ssSetDWorkDataType(S, 67, SS_POINTER);

32522


32523
  /* '<S6>/Scope50': PWORK */

32524
  ssSetDWorkName(S, 68, "DWORK68");

32525
  ssSetDWorkWidth(S, 68, 1);

32526
  ssSetDWorkDataType(S, 68, SS_POINTER);

32527


32528
  /* '<S6>/Scope51': PWORK */

32529
  ssSetDWorkName(S, 69, "DWORK69");

32530
  ssSetDWorkWidth(S, 69, 1);

32531
  ssSetDWorkDataType(S, 69, SS_POINTER);

32532


32533
  /* '<S6>/Scope52': PWORK */

32534
  ssSetDWorkName(S, 70, "DWORK70");

32535
  ssSetDWorkWidth(S, 70, 1);

32536
  ssSetDWorkDataType(S, 70, SS_POINTER);

32537


32538
  /* '<S6>/Scope53': PWORK */

32539
  ssSetDWorkName(S, 71, "DWORK71");

32540
  ssSetDWorkWidth(S, 71, 1);

32541
  ssSetDWorkDataType(S, 71, SS_POINTER);

32542


32543
  /* '<S6>/Scope54': PWORK */

32544
  ssSetDWorkName(S, 72, "DWORK72");

32545
  ssSetDWorkWidth(S, 72, 1);

32546
  ssSetDWorkDataType(S, 72, SS_POINTER);

32547


32548
  /* '<S6>/Scope55': PWORK */

32549
  ssSetDWorkName(S, 73, "DWORK73");

32550
  ssSetDWorkWidth(S, 73, 1);

32551
  ssSetDWorkDataType(S, 73, SS_POINTER);

32552


32553
  /* '<S6>/Scope56': PWORK */

32554
  ssSetDWorkName(S, 74, "DWORK74");

32555
  ssSetDWorkWidth(S, 74, 1);

32556
  ssSetDWorkDataType(S, 74, SS_POINTER);

32557


32558
  /* '<S6>/Scope57': PWORK */

32559
  ssSetDWorkName(S, 75, "DWORK75");

32560
  ssSetDWorkWidth(S, 75, 1);

32561
  ssSetDWorkDataType(S, 75, SS_POINTER);

32562


32563
  /* '<S6>/Scope58': PWORK */

32564
  ssSetDWorkName(S, 76, "DWORK76");

32565
  ssSetDWorkWidth(S, 76, 1);

32566
  ssSetDWorkDataType(S, 76, SS_POINTER);

32567


32568
  /* '<S6>/Scope59': PWORK */

32569
  ssSetDWorkName(S, 77, "DWORK77");

32570
  ssSetDWorkWidth(S, 77, 1);

32571
  ssSetDWorkDataType(S, 77, SS_POINTER);

32572


32573
  /* '<S6>/Scope6': PWORK */

32574
  ssSetDWorkName(S, 78, "DWORK78");

32575
  ssSetDWorkWidth(S, 78, 1);

32576
  ssSetDWorkDataType(S, 78, SS_POINTER);

32577


32578
  /* '<S6>/Scope60': PWORK */

32579
  ssSetDWorkName(S, 79, "DWORK79");

32580
  ssSetDWorkWidth(S, 79, 1);

32581
  ssSetDWorkDataType(S, 79, SS_POINTER);

32582


32583
  /* '<S6>/Scope61': PWORK */

32584
  ssSetDWorkName(S, 80, "DWORK80");

32585
  ssSetDWorkWidth(S, 80, 2);

32586
  ssSetDWorkDataType(S, 80, SS_POINTER);

32587


32588
  /* '<S6>/Scope62': PWORK */

32589
  ssSetDWorkName(S, 81, "DWORK81");

32590
  ssSetDWorkWidth(S, 81, 1);

32591
  ssSetDWorkDataType(S, 81, SS_POINTER);

32592


32593
  /* '<S6>/Scope63': PWORK */

32594
  ssSetDWorkName(S, 82, "DWORK82");

32595
  ssSetDWorkWidth(S, 82, 1);

32596
  ssSetDWorkDataType(S, 82, SS_POINTER);

32597


32598
  /* '<S6>/Scope64': PWORK */

32599
  ssSetDWorkName(S, 83, "DWORK83");

32600
  ssSetDWorkWidth(S, 83, 1);

32601
  ssSetDWorkDataType(S, 83, SS_POINTER);

32602


32603
  /* '<S6>/Scope65': PWORK */

32604
  ssSetDWorkName(S, 84, "DWORK84");

32605
  ssSetDWorkWidth(S, 84, 1);

32606
  ssSetDWorkDataType(S, 84, SS_POINTER);

32607


32608
  /* '<S6>/Scope66': PWORK */

32609
  ssSetDWorkName(S, 85, "DWORK85");

32610
  ssSetDWorkWidth(S, 85, 1);

32611
  ssSetDWorkDataType(S, 85, SS_POINTER);

32612


32613
  /* '<S6>/Scope69': PWORK */

32614
  ssSetDWorkName(S, 86, "DWORK86");

32615
  ssSetDWorkWidth(S, 86, 1);

32616
  ssSetDWorkDataType(S, 86, SS_POINTER);

32617


32618
  /* '<S6>/Scope7': PWORK */

32619
  ssSetDWorkName(S, 87, "DWORK87");

32620
  ssSetDWorkWidth(S, 87, 1);

32621
  ssSetDWorkDataType(S, 87, SS_POINTER);

32622


32623
  /* '<S6>/Scope70': PWORK */

32624
  ssSetDWorkName(S, 88, "DWORK88");

32625
  ssSetDWorkWidth(S, 88, 1);

32626
  ssSetDWorkDataType(S, 88, SS_POINTER);

32627


32628
  /* '<S6>/Scope71': PWORK */

32629
  ssSetDWorkName(S, 89, "DWORK89");

32630
  ssSetDWorkWidth(S, 89, 3);

32631
  ssSetDWorkDataType(S, 89, SS_POINTER);

32632


32633
  /* '<S6>/Scope72': PWORK */

32634
  ssSetDWorkName(S, 90, "DWORK90");

32635
  ssSetDWorkWidth(S, 90, 1);

32636
  ssSetDWorkDataType(S, 90, SS_POINTER);

32637


32638
  /* '<S6>/Scope73': PWORK */

32639
  ssSetDWorkName(S, 91, "DWORK91");

32640
  ssSetDWorkWidth(S, 91, 1);

32641
  ssSetDWorkDataType(S, 91, SS_POINTER);

32642


32643
  /* '<S6>/Scope75': PWORK */

32644
  ssSetDWorkName(S, 92, "DWORK92");

32645
  ssSetDWorkWidth(S, 92, 1);

32646
  ssSetDWorkDataType(S, 92, SS_POINTER);

32647


32648
  /* '<S6>/Scope8': PWORK */

32649
  ssSetDWorkName(S, 93, "DWORK93");

32650
  ssSetDWorkWidth(S, 93, 1);

32651
  ssSetDWorkDataType(S, 93, SS_POINTER);

32652


32653
  /* '<S6>/Scope9': PWORK */

32654
  ssSetDWorkName(S, 94, "DWORK94");

32655
  ssSetDWorkWidth(S, 94, 1);

32656
  ssSetDWorkDataType(S, 94, SS_POINTER);

32657


32658
  /* '<S6>/Vc': PWORK */

32659
  ssSetDWorkName(S, 95, "DWORK95");

32660
  ssSetDWorkWidth(S, 95, 1);

32661
  ssSetDWorkDataType(S, 95, SS_POINTER);

32662


32663
  /* '<S6>/alfa': PWORK */

32664
  ssSetDWorkName(S, 96, "DWORK96");

32665
  ssSetDWorkWidth(S, 96, 1);

32666
  ssSetDWorkDataType(S, 96, SS_POINTER);

32667


32668
  /* '<S6>/alfa1': PWORK */

32669
  ssSetDWorkName(S, 97, "DWORK97");

32670
  ssSetDWorkWidth(S, 97, 1);

32671
  ssSetDWorkDataType(S, 97, SS_POINTER);

32672


32673
  /* '<S6>/alfa2': PWORK */

32674
  ssSetDWorkName(S, 98, "DWORK98");

32675
  ssSetDWorkWidth(S, 98, 1);

32676
  ssSetDWorkDataType(S, 98, SS_POINTER);

32677


32678
  /* '<S6>/alfa4': PWORK */

32679
  ssSetDWorkName(S, 99, "DWORK99");

32680
  ssSetDWorkWidth(S, 99, 1);

32681
  ssSetDWorkDataType(S, 99, SS_POINTER);

32682


32683
  /* '<S6>/alfa5': PWORK */

32684
  ssSetDWorkName(S, 100, "DWORK100");

32685
  ssSetDWorkWidth(S, 100, 1);

32686
  ssSetDWorkDataType(S, 100, SS_POINTER);

32687


32688
  /* '<S6>/beta': PWORK */

32689
  ssSetDWorkName(S, 101, "DWORK101");

32690
  ssSetDWorkWidth(S, 101, 1);

32691
  ssSetDWorkDataType(S, 101, SS_POINTER);

32692


32693
  /* '<S6>/k': PWORK */

32694
  ssSetDWorkName(S, 102, "DWORK102");

32695
  ssSetDWorkWidth(S, 102, 1);

32696
  ssSetDWorkDataType(S, 102, SS_POINTER);

32697


32698
  /* '<S6>/k1': PWORK */

32699
  ssSetDWorkName(S, 103, "DWORK103");

32700
  ssSetDWorkWidth(S, 103, 1);

32701
  ssSetDWorkDataType(S, 103, SS_POINTER);

32702


32703
  /* '<S6>/psd': PWORK */

32704
  ssSetDWorkName(S, 104, "DWORK104");

32705
  ssSetDWorkWidth(S, 104, 1);

32706
  ssSetDWorkDataType(S, 104, SS_POINTER);

32707


32708
  /* '<S6>/thetaL1': PWORK */

32709
  ssSetDWorkName(S, 105, "DWORK105");

32710
  ssSetDWorkWidth(S, 105, 1);

32711
  ssSetDWorkDataType(S, 105, SS_POINTER);

32712


32713
  /* '<S6>/thetaL2': PWORK */

32714
  ssSetDWorkName(S, 106, "DWORK106");

32715
  ssSetDWorkWidth(S, 106, 1);

32716
  ssSetDWorkDataType(S, 106, SS_POINTER);

32717


32718
  /* '<S1>/Scope': PWORK */

32719
  ssSetDWorkName(S, 107, "DWORK107");

32720
  ssSetDWorkWidth(S, 107, 1);

32721
  ssSetDWorkDataType(S, 107, SS_POINTER);

32722


32723
  /* '<S1>/Scope1': PWORK */

32724
  ssSetDWorkName(S, 108, "DWORK108");

32725
  ssSetDWorkWidth(S, 108, 1);

32726
  ssSetDWorkDataType(S, 108, SS_POINTER);

32727


32728
  /* '<S1>/Scope2': PWORK */

32729
  ssSetDWorkName(S, 109, "DWORK109");

32730
  ssSetDWorkWidth(S, 109, 1);

32731
  ssSetDWorkDataType(S, 109, SS_POINTER);

32732


32733
  /* '<S1>/To Workspace': PWORK */

32734
  ssSetDWorkName(S, 110, "DWORK110");

32735
  ssSetDWorkWidth(S, 110, 1);

32736
  ssSetDWorkDataType(S, 110, SS_POINTER);

32737


32738
  /* '<S31>/Scope': PWORK */

32739
  ssSetDWorkName(S, 111, "DWORK111");

32740
  ssSetDWorkWidth(S, 111, 1);

32741
  ssSetDWorkDataType(S, 111, SS_POINTER);

32742


32743
  /* '<S9>/Scope': PWORK */

32744
  ssSetDWorkName(S, 112, "DWORK112");

32745
  ssSetDWorkWidth(S, 112, 1);

32746
  ssSetDWorkDataType(S, 112, SS_POINTER);

32747


32748
  /* '<S9>/Scope1': PWORK */

32749
  ssSetDWorkName(S, 113, "DWORK113");

32750
  ssSetDWorkWidth(S, 113, 1);

32751
  ssSetDWorkDataType(S, 113, SS_POINTER);

32752


32753
  /* '<S9>/Scope2': PWORK */

32754
  ssSetDWorkName(S, 114, "DWORK114");

32755
  ssSetDWorkWidth(S, 114, 1);

32756
  ssSetDWorkDataType(S, 114, SS_POINTER);

32757


32758
  /* '<S9>/Scope3': PWORK */

32759
  ssSetDWorkName(S, 115, "DWORK115");

32760
  ssSetDWorkWidth(S, 115, 1);

32761
  ssSetDWorkDataType(S, 115, SS_POINTER);

32762


32763
  /* '<S9>/Scope4': PWORK */

32764
  ssSetDWorkName(S, 116, "DWORK116");

32765
  ssSetDWorkWidth(S, 116, 1);

32766
  ssSetDWorkDataType(S, 116, SS_POINTER);

32767


32768
  /* '<S9>/Scope5': PWORK */

32769
  ssSetDWorkName(S, 117, "DWORK117");

32770
  ssSetDWorkWidth(S, 117, 1);

32771
  ssSetDWorkDataType(S, 117, SS_POINTER);

32772


32773
  /* '<S9>/Scope7': PWORK */

32774
  ssSetDWorkName(S, 118, "DWORK118");

32775
  ssSetDWorkWidth(S, 118, 1);

32776
  ssSetDWorkDataType(S, 118, SS_POINTER);

32777


32778
  /* '<S9>/To Workspace': PWORK */

32779
  ssSetDWorkName(S, 119, "DWORK119");

32780
  ssSetDWorkWidth(S, 119, 1);

32781
  ssSetDWorkDataType(S, 119, SS_POINTER);

32782


32783
  /* '<S9>/To Workspace1': PWORK */

32784
  ssSetDWorkName(S, 120, "DWORK120");

32785
  ssSetDWorkWidth(S, 120, 1);

32786
  ssSetDWorkDataType(S, 120, SS_POINTER);

32787


32788
  /* '<S15>/Scope26': PWORK */

32789
  ssSetDWorkName(S, 121, "DWORK121");

32790
  ssSetDWorkWidth(S, 121, 3);

32791
  ssSetDWorkDataType(S, 121, SS_POINTER);

32792


32793
  /* '<S448>/Scope': PWORK */

32794
  ssSetDWorkName(S, 122, "DWORK122");

32795
  ssSetDWorkWidth(S, 122, 1);

32796
  ssSetDWorkDataType(S, 122, SS_POINTER);

32797


32798
  /* '<S449>/Scope': PWORK */

32799
  ssSetDWorkName(S, 123, "DWORK123");

32800
  ssSetDWorkWidth(S, 123, 1);

32801
  ssSetDWorkDataType(S, 123, SS_POINTER);

32802


32803
  /* '<S480>/Scope2': PWORK */

32804
  ssSetDWorkName(S, 124, "DWORK124");

32805
  ssSetDWorkWidth(S, 124, 1);

32806
  ssSetDWorkDataType(S, 124, SS_POINTER);

32807


32808
  /* '<S466>/Scope9': PWORK */

32809
  ssSetDWorkName(S, 125, "DWORK125");

32810
  ssSetDWorkWidth(S, 125, 1);

32811
  ssSetDWorkDataType(S, 125, SS_POINTER);

32812


32813
  /* '<S617>/Scope1': PWORK */

32814
  ssSetDWorkName(S, 126, "DWORK126");

32815
  ssSetDWorkWidth(S, 126, 1);

32816
  ssSetDWorkDataType(S, 126, SS_POINTER);

32817


32818
  /* '<S617>/Scope10': PWORK */

32819
  ssSetDWorkName(S, 127, "DWORK127");

32820
  ssSetDWorkWidth(S, 127, 1);

32821
  ssSetDWorkDataType(S, 127, SS_POINTER);

32822


32823
  /* '<S617>/Scope11': PWORK */

32824
  ssSetDWorkName(S, 128, "DWORK128");

32825
  ssSetDWorkWidth(S, 128, 1);

32826
  ssSetDWorkDataType(S, 128, SS_POINTER);

32827


32828
  /* '<S617>/Scope12': PWORK */

32829
  ssSetDWorkName(S, 129, "DWORK129");

32830
  ssSetDWorkWidth(S, 129, 1);

32831
  ssSetDWorkDataType(S, 129, SS_POINTER);

32832


32833
  /* '<S617>/Scope13': PWORK */

32834
  ssSetDWorkName(S, 130, "DWORK130");

32835
  ssSetDWorkWidth(S, 130, 1);

32836
  ssSetDWorkDataType(S, 130, SS_POINTER);

32837


32838
  /* '<S617>/Scope2': PWORK */

32839
  ssSetDWorkName(S, 131, "DWORK131");

32840
  ssSetDWorkWidth(S, 131, 2);

32841
  ssSetDWorkDataType(S, 131, SS_POINTER);

32842


32843
  /* '<S617>/Scope3': PWORK */

32844
  ssSetDWorkName(S, 132, "DWORK132");

32845
  ssSetDWorkWidth(S, 132, 2);

32846
  ssSetDWorkDataType(S, 132, SS_POINTER);

32847


32848
  /* '<S617>/Scope4': PWORK */

32849
  ssSetDWorkName(S, 133, "DWORK133");

32850
  ssSetDWorkWidth(S, 133, 1);

32851
  ssSetDWorkDataType(S, 133, SS_POINTER);

32852


32853
  /* '<S617>/Scope5': PWORK */

32854
  ssSetDWorkName(S, 134, "DWORK134");

32855
  ssSetDWorkWidth(S, 134, 1);

32856
  ssSetDWorkDataType(S, 134, SS_POINTER);

32857


32858
  /* '<S617>/Scope6': PWORK */

32859
  ssSetDWorkName(S, 135, "DWORK135");

32860
  ssSetDWorkWidth(S, 135, 1);

32861
  ssSetDWorkDataType(S, 135, SS_POINTER);

32862


32863
  /* '<S617>/Scope7': PWORK */

32864
  ssSetDWorkName(S, 136, "DWORK136");

32865
  ssSetDWorkWidth(S, 136, 1);

32866
  ssSetDWorkDataType(S, 136, SS_POINTER);

32867


32868
  /* '<S617>/Scope8': PWORK */

32869
  ssSetDWorkName(S, 137, "DWORK137");

32870
  ssSetDWorkWidth(S, 137, 1);

32871
  ssSetDWorkDataType(S, 137, SS_POINTER);

32872


32873
  /* '<S617>/Scope9': PWORK */

32874
  ssSetDWorkName(S, 138, "DWORK138");

32875
  ssSetDWorkWidth(S, 138, 1);

32876
  ssSetDWorkDataType(S, 138, SS_POINTER);

32877


32878
  /* '<S619>/Scope8': PWORK */

32879
  ssSetDWorkName(S, 139, "DWORK139");

32880
  ssSetDWorkWidth(S, 139, 3);

32881
  ssSetDWorkDataType(S, 139, SS_POINTER);

32882


32883
  /* '<S682>/Scope2': PWORK */

32884
  ssSetDWorkName(S, 140, "DWORK140");

32885
  ssSetDWorkWidth(S, 140, 1);

32886
  ssSetDWorkDataType(S, 140, SS_POINTER);

32887


32888
  /* '<S682>/Scope1': PWORK */

32889
  ssSetDWorkName(S, 141, "DWORK141");

32890
  ssSetDWorkWidth(S, 141, 1);

32891
  ssSetDWorkDataType(S, 141, SS_POINTER);

32892


32893
  /* '<S683>/Scope8': PWORK */

32894
  ssSetDWorkName(S, 142, "DWORK142");

32895
  ssSetDWorkWidth(S, 142, 3);

32896
  ssSetDWorkDataType(S, 142, SS_POINTER);

32897


32898
  /* '<S335>/Scope1': PWORK */

32899
  ssSetDWorkName(S, 143, "DWORK143");

32900
  ssSetDWorkWidth(S, 143, 1);

32901
  ssSetDWorkDataType(S, 143, SS_POINTER);

32902


32903
  /* '<S335>/To Workspace': PWORK */

32904
  ssSetDWorkName(S, 144, "DWORK144");

32905
  ssSetDWorkWidth(S, 144, 1);

32906
  ssSetDWorkDataType(S, 144, SS_POINTER);

32907


32908
  /* '<S315>/Scope': PWORK */

32909
  ssSetDWorkName(S, 145, "DWORK145");

32910
  ssSetDWorkWidth(S, 145, 1);

32911
  ssSetDWorkDataType(S, 145, SS_POINTER);

32912


32913
  /* '<S175>/Unit Delay1': DSTATE */

32914
  ssSetDWorkName(S, 146, "DWORK146");

32915
  ssSetDWorkWidth(S, 146, 1);

32916
  ssSetDWorkDataType(S, 146, SS_SINGLE);

32917
  ssSetDWorkUsedAsDState(S, 146, 1);

32918


32919
  /* '<S174>/Generated S-Function': DWORK0 */

32920
  ssSetDWorkName(S, 147, "DWORK147");

32921
  ssSetDWorkWidth(S, 147, 1);

32922
  ssSetDWorkDataType(S, 147, SS_SINGLE);

32923
  ssSetDWorkUsedAsDState(S, 147, 1);

32924


32925
  /* '<S174>/Generated S-Function': DWORK1 */

32926
  ssSetDWorkName(S, 148, "DWORK148");

32927
  ssSetDWorkWidth(S, 148, 1);

32928
  ssSetDWorkDataType(S, 148, SS_SINGLE);

32929
  ssSetDWorkUsedAsDState(S, 148, 1);

32930


32931
  /* '<S185>/Generated S-Function': DWORK0 */

32932
  ssSetDWorkName(S, 149, "DWORK149");

32933
  ssSetDWorkWidth(S, 149, 1);

32934
  ssSetDWorkDataType(S, 149, SS_SINGLE);

32935
  ssSetDWorkUsedAsDState(S, 149, 1);

32936


32937
  /* '<S185>/Generated S-Function': DWORK1 */

32938
  ssSetDWorkName(S, 150, "DWORK150");

32939
  ssSetDWorkWidth(S, 150, 1);

32940
  ssSetDWorkDataType(S, 150, SS_SINGLE);

32941
  ssSetDWorkUsedAsDState(S, 150, 1);

32942


32943
  /* '<S184>/Generated S-Function': DWORK0 */

32944
  ssSetDWorkName(S, 151, "DWORK151");

32945
  ssSetDWorkWidth(S, 151, 1);

32946
  ssSetDWorkDataType(S, 151, SS_SINGLE);

32947
  ssSetDWorkUsedAsDState(S, 151, 1);

32948


32949
  /* '<S184>/Generated S-Function': DWORK1 */

32950
  ssSetDWorkName(S, 152, "DWORK152");

32951
  ssSetDWorkWidth(S, 152, 1);

32952
  ssSetDWorkDataType(S, 152, SS_SINGLE);

32953
  ssSetDWorkUsedAsDState(S, 152, 1);

32954


32955
  /* '<S183>/Generated S-Function': DWORK0 */

32956
  ssSetDWorkName(S, 153, "DWORK153");

32957
  ssSetDWorkWidth(S, 153, 1);

32958
  ssSetDWorkDataType(S, 153, SS_SINGLE);

32959
  ssSetDWorkUsedAsDState(S, 153, 1);

32960


32961
  /* '<S183>/Generated S-Function': DWORK1 */

32962
  ssSetDWorkName(S, 154, "DWORK154");

32963
  ssSetDWorkWidth(S, 154, 1);

32964
  ssSetDWorkDataType(S, 154, SS_SINGLE);

32965
  ssSetDWorkUsedAsDState(S, 154, 1);

32966


32967
  /* '<S178>/Generated S-Function': DWORK0 */

32968
  ssSetDWorkName(S, 155, "DWORK155");

32969
  ssSetDWorkWidth(S, 155, 1);

32970
  ssSetDWorkDataType(S, 155, SS_SINGLE);

32971
  ssSetDWorkUsedAsDState(S, 155, 1);

32972


32973
  /* '<S178>/Generated S-Function': DWORK1 */

32974
  ssSetDWorkName(S, 156, "DWORK156");

32975
  ssSetDWorkWidth(S, 156, 1);

32976
  ssSetDWorkDataType(S, 156, SS_SINGLE);

32977
  ssSetDWorkUsedAsDState(S, 156, 1);

32978


32979
  /* '<S177>/Generated S-Function': DWORK0 */

32980
  ssSetDWorkName(S, 157, "DWORK157");

32981
  ssSetDWorkWidth(S, 157, 1);

32982
  ssSetDWorkDataType(S, 157, SS_SINGLE);

32983
  ssSetDWorkUsedAsDState(S, 157, 1);

32984


32985
  /* '<S177>/Generated S-Function': DWORK1 */

32986
  ssSetDWorkName(S, 158, "DWORK158");

32987
  ssSetDWorkWidth(S, 158, 1);

32988
  ssSetDWorkDataType(S, 158, SS_SINGLE);

32989
  ssSetDWorkUsedAsDState(S, 158, 1);

32990


32991
  /* '<S176>/Generated S-Function': DWORK0 */

32992
  ssSetDWorkName(S, 159, "DWORK159");

32993
  ssSetDWorkWidth(S, 159, 1);

32994
  ssSetDWorkDataType(S, 159, SS_SINGLE);

32995
  ssSetDWorkUsedAsDState(S, 159, 1);

32996


32997
  /* '<S176>/Generated S-Function': DWORK1 */

32998
  ssSetDWorkName(S, 160, "DWORK160");

32999
  ssSetDWorkWidth(S, 160, 1);

33000
  ssSetDWorkDataType(S, 160, SS_SINGLE);

33001
  ssSetDWorkUsedAsDState(S, 160, 1);

33002


33003
  /* '<S115>/Unit Delay1': DSTATE */

33004
  ssSetDWorkName(S, 161, "DWORK161");

33005
  ssSetDWorkWidth(S, 161, 1);

33006
  ssSetDWorkDataType(S, 161, SS_SINGLE);

33007
  ssSetDWorkUsedAsDState(S, 161, 1);

33008


33009
  /* '<S89>/Generated S-Function3': DWORK0 */

33010
  ssSetDWorkName(S, 162, "DWORK162");

33011
  ssSetDWorkWidth(S, 162, 1);

33012
  ssSetDWorkDataType(S, 162, SS_SINGLE);

33013
  ssSetDWorkUsedAsDState(S, 162, 1);

33014


33015
  /* '<S89>/Generated S-Function3': DWORK1 */

33016
  ssSetDWorkName(S, 163, "DWORK163");

33017
  ssSetDWorkWidth(S, 163, 1);

33018
  ssSetDWorkDataType(S, 163, SS_SINGLE);

33019
  ssSetDWorkUsedAsDState(S, 163, 1);

33020


33021
  /* '<S90>/Generated S-Function2': DWORK0 */

33022
  ssSetDWorkName(S, 164, "DWORK164");

33023
  ssSetDWorkWidth(S, 164, 1);

33024
  ssSetDWorkDataType(S, 164, SS_SINGLE);

33025
  ssSetDWorkUsedAsDState(S, 164, 1);

33026


33027
  /* '<S90>/Generated S-Function2': DWORK1 */

33028
  ssSetDWorkName(S, 165, "DWORK165");

33029
  ssSetDWorkWidth(S, 165, 1);

33030
  ssSetDWorkDataType(S, 165, SS_SINGLE);

33031
  ssSetDWorkUsedAsDState(S, 165, 1);

33032


33033
  /* '<S87>/Generated S-Function4': DWORK0 */

33034
  ssSetDWorkName(S, 166, "DWORK166");

33035
  ssSetDWorkWidth(S, 166, 1);

33036
  ssSetDWorkDataType(S, 166, SS_SINGLE);

33037
  ssSetDWorkUsedAsDState(S, 166, 1);

33038


33039
  /* '<S87>/Generated S-Function4': DWORK1 */

33040
  ssSetDWorkName(S, 167, "DWORK167");

33041
  ssSetDWorkWidth(S, 167, 1);

33042
  ssSetDWorkDataType(S, 167, SS_SINGLE);

33043
  ssSetDWorkUsedAsDState(S, 167, 1);

33044


33045
  /* '<S113>/Generated S-Function': DWORK0 */

33046
  ssSetDWorkName(S, 168, "DWORK168");

33047
  ssSetDWorkWidth(S, 168, 1);

33048
  ssSetDWorkDataType(S, 168, SS_SINGLE);

33049
  ssSetDWorkUsedAsDState(S, 168, 1);

33050


33051
  /* '<S113>/Generated S-Function': DWORK1 */

33052
  ssSetDWorkName(S, 169, "DWORK169");

33053
  ssSetDWorkWidth(S, 169, 1);

33054
  ssSetDWorkDataType(S, 169, SS_SINGLE);

33055
  ssSetDWorkUsedAsDState(S, 169, 1);

33056


33057
  /* '<S118>/Generated S-Function2': DWORK0 */

33058
  ssSetDWorkName(S, 170, "DWORK170");

33059
  ssSetDWorkWidth(S, 170, 1);

33060
  ssSetDWorkDataType(S, 170, SS_SINGLE);

33061
  ssSetDWorkUsedAsDState(S, 170, 1);

33062


33063
  /* '<S118>/Generated S-Function2': DWORK1 */

33064
  ssSetDWorkName(S, 171, "DWORK171");

33065
  ssSetDWorkWidth(S, 171, 1);

33066
  ssSetDWorkDataType(S, 171, SS_SINGLE);

33067
  ssSetDWorkUsedAsDState(S, 171, 1);

33068


33069
  /* '<S117>/Generated S-Function4': DWORK0 */

33070
  ssSetDWorkName(S, 172, "DWORK172");

33071
  ssSetDWorkWidth(S, 172, 1);

33072
  ssSetDWorkDataType(S, 172, SS_SINGLE);

33073
  ssSetDWorkUsedAsDState(S, 172, 1);

33074


33075
  /* '<S117>/Generated S-Function4': DWORK1 */

33076
  ssSetDWorkName(S, 173, "DWORK173");

33077
  ssSetDWorkWidth(S, 173, 1);

33078
  ssSetDWorkDataType(S, 173, SS_SINGLE);

33079
  ssSetDWorkUsedAsDState(S, 173, 1);

33080


33081
  /* '<S116>/Generated S-Function4': DWORK0 */

33082
  ssSetDWorkName(S, 174, "DWORK174");

33083
  ssSetDWorkWidth(S, 174, 1);

33084
  ssSetDWorkDataType(S, 174, SS_SINGLE);

33085
  ssSetDWorkUsedAsDState(S, 174, 1);

33086


33087
  /* '<S116>/Generated S-Function4': DWORK1 */

33088
  ssSetDWorkName(S, 175, "DWORK175");

33089
  ssSetDWorkWidth(S, 175, 1);

33090
  ssSetDWorkDataType(S, 175, SS_SINGLE);

33091
  ssSetDWorkUsedAsDState(S, 175, 1);

33092


33093
  /* '<S100>/Generated S-Function3': DWORK0 */

33094
  ssSetDWorkName(S, 176, "DWORK176");

33095
  ssSetDWorkWidth(S, 176, 1);

33096
  ssSetDWorkDataType(S, 176, SS_SINGLE);

33097
  ssSetDWorkUsedAsDState(S, 176, 1);

33098


33099
  /* '<S100>/Generated S-Function3': DWORK1 */

33100
  ssSetDWorkName(S, 177, "DWORK177");

33101
  ssSetDWorkWidth(S, 177, 1);

33102
  ssSetDWorkDataType(S, 177, SS_SINGLE);

33103
  ssSetDWorkUsedAsDState(S, 177, 1);

33104


33105
  /* '<S101>/Generated S-Function2': DWORK0 */

33106
  ssSetDWorkName(S, 178, "DWORK178");

33107
  ssSetDWorkWidth(S, 178, 1);

33108
  ssSetDWorkDataType(S, 178, SS_SINGLE);

33109
  ssSetDWorkUsedAsDState(S, 178, 1);

33110


33111
  /* '<S101>/Generated S-Function2': DWORK1 */

33112
  ssSetDWorkName(S, 179, "DWORK179");

33113
  ssSetDWorkWidth(S, 179, 1);

33114
  ssSetDWorkDataType(S, 179, SS_SINGLE);

33115
  ssSetDWorkUsedAsDState(S, 179, 1);

33116


33117
  /* '<S98>/Generated S-Function4': DWORK0 */

33118
  ssSetDWorkName(S, 180, "DWORK180");

33119
  ssSetDWorkWidth(S, 180, 1);

33120
  ssSetDWorkDataType(S, 180, SS_SINGLE);

33121
  ssSetDWorkUsedAsDState(S, 180, 1);

33122


33123
  /* '<S98>/Generated S-Function4': DWORK1 */

33124
  ssSetDWorkName(S, 181, "DWORK181");

33125
  ssSetDWorkWidth(S, 181, 1);

33126
  ssSetDWorkDataType(S, 181, SS_SINGLE);

33127
  ssSetDWorkUsedAsDState(S, 181, 1);

33128


33129
  /* '<S107>/Generated S-Function3': DWORK0 */

33130
  ssSetDWorkName(S, 182, "DWORK182");

33131
  ssSetDWorkWidth(S, 182, 1);

33132
  ssSetDWorkDataType(S, 182, SS_SINGLE);

33133
  ssSetDWorkUsedAsDState(S, 182, 1);

33134


33135
  /* '<S107>/Generated S-Function3': DWORK1 */

33136
  ssSetDWorkName(S, 183, "DWORK183");

33137
  ssSetDWorkWidth(S, 183, 1);

33138
  ssSetDWorkDataType(S, 183, SS_SINGLE);

33139
  ssSetDWorkUsedAsDState(S, 183, 1);

33140


33141
  /* '<S108>/Generated S-Function2': DWORK0 */

33142
  ssSetDWorkName(S, 184, "DWORK184");

33143
  ssSetDWorkWidth(S, 184, 1);

33144
  ssSetDWorkDataType(S, 184, SS_SINGLE);

33145
  ssSetDWorkUsedAsDState(S, 184, 1);

33146


33147
  /* '<S108>/Generated S-Function2': DWORK1 */

33148
  ssSetDWorkName(S, 185, "DWORK185");

33149
  ssSetDWorkWidth(S, 185, 1);

33150
  ssSetDWorkDataType(S, 185, SS_SINGLE);

33151
  ssSetDWorkUsedAsDState(S, 185, 1);

33152


33153
  /* '<S106>/Generated S-Function4': DWORK0 */

33154
  ssSetDWorkName(S, 186, "DWORK186");

33155
  ssSetDWorkWidth(S, 186, 1);

33156
  ssSetDWorkDataType(S, 186, SS_SINGLE);

33157
  ssSetDWorkUsedAsDState(S, 186, 1);

33158


33159
  /* '<S106>/Generated S-Function4': DWORK1 */

33160
  ssSetDWorkName(S, 187, "DWORK187");

33161
  ssSetDWorkWidth(S, 187, 1);

33162
  ssSetDWorkDataType(S, 187, SS_SINGLE);

33163
  ssSetDWorkUsedAsDState(S, 187, 1);

33164


33165
  /* '<S138>/Unit Delay1': DSTATE */

33166
  ssSetDWorkName(S, 188, "DWORK188");

33167
  ssSetDWorkWidth(S, 188, 1);

33168
  ssSetDWorkDataType(S, 188, SS_SINGLE);

33169
  ssSetDWorkUsedAsDState(S, 188, 1);

33170


33171
  /* '<S130>/Generated S-Function4': DWORK0 */

33172
  ssSetDWorkName(S, 189, "DWORK189");

33173
  ssSetDWorkWidth(S, 189, 1);

33174
  ssSetDWorkDataType(S, 189, SS_SINGLE);

33175
  ssSetDWorkUsedAsDState(S, 189, 1);

33176


33177
  /* '<S130>/Generated S-Function4': DWORK1 */

33178
  ssSetDWorkName(S, 190, "DWORK190");

33179
  ssSetDWorkWidth(S, 190, 1);

33180
  ssSetDWorkDataType(S, 190, SS_SINGLE);

33181
  ssSetDWorkUsedAsDState(S, 190, 1);

33182


33183
  /* '<S125>/Unit Delay1': DSTATE */

33184
  ssSetDWorkName(S, 191, "DWORK191");

33185
  ssSetDWorkWidth(S, 191, 1);

33186
  ssSetDWorkDataType(S, 191, SS_SINGLE);

33187
  ssSetDWorkUsedAsDState(S, 191, 1);

33188


33189
  /* '<S132>/Generated S-Function3': DWORK0 */

33190
  ssSetDWorkName(S, 192, "DWORK192");

33191
  ssSetDWorkWidth(S, 192, 1);

33192
  ssSetDWorkDataType(S, 192, SS_SINGLE);

33193
  ssSetDWorkUsedAsDState(S, 192, 1);

33194


33195
  /* '<S132>/Generated S-Function3': DWORK1 */

33196
  ssSetDWorkName(S, 193, "DWORK193");

33197
  ssSetDWorkWidth(S, 193, 1);

33198
  ssSetDWorkDataType(S, 193, SS_SINGLE);

33199
  ssSetDWorkUsedAsDState(S, 193, 1);

33200


33201
  /* '<S133>/Generated S-Function2': DWORK0 */

33202
  ssSetDWorkName(S, 194, "DWORK194");

33203
  ssSetDWorkWidth(S, 194, 1);

33204
  ssSetDWorkDataType(S, 194, SS_SINGLE);

33205
  ssSetDWorkUsedAsDState(S, 194, 1);

33206


33207
  /* '<S133>/Generated S-Function2': DWORK1 */

33208
  ssSetDWorkName(S, 195, "DWORK195");

33209
  ssSetDWorkWidth(S, 195, 1);

33210
  ssSetDWorkDataType(S, 195, SS_SINGLE);

33211
  ssSetDWorkUsedAsDState(S, 195, 1);

33212


33213
  /* '<S131>/Generated S-Function4': DWORK0 */

33214
  ssSetDWorkName(S, 196, "DWORK196");

33215
  ssSetDWorkWidth(S, 196, 1);

33216
  ssSetDWorkDataType(S, 196, SS_SINGLE);

33217
  ssSetDWorkUsedAsDState(S, 196, 1);

33218


33219
  /* '<S131>/Generated S-Function4': DWORK1 */

33220
  ssSetDWorkName(S, 197, "DWORK197");

33221
  ssSetDWorkWidth(S, 197, 1);

33222
  ssSetDWorkDataType(S, 197, SS_SINGLE);

33223
  ssSetDWorkUsedAsDState(S, 197, 1);

33224


33225
  /* '<S152>/Generated S-Function2': DWORK0 */

33226
  ssSetDWorkName(S, 198, "DWORK198");

33227
  ssSetDWorkWidth(S, 198, 1);

33228
  ssSetDWorkDataType(S, 198, SS_SINGLE);

33229
  ssSetDWorkUsedAsDState(S, 198, 1);

33230


33231
  /* '<S152>/Generated S-Function2': DWORK1 */

33232
  ssSetDWorkName(S, 199, "DWORK199");

33233
  ssSetDWorkWidth(S, 199, 1);

33234
  ssSetDWorkDataType(S, 199, SS_SINGLE);

33235
  ssSetDWorkUsedAsDState(S, 199, 1);

33236


33237
  /* '<S150>/Generated S-Function3': DWORK0 */

33238
  ssSetDWorkName(S, 200, "DWORK200");

33239
  ssSetDWorkWidth(S, 200, 1);

33240
  ssSetDWorkDataType(S, 200, SS_SINGLE);

33241
  ssSetDWorkUsedAsDState(S, 200, 1);

33242


33243
  /* '<S150>/Generated S-Function3': DWORK1 */

33244
  ssSetDWorkName(S, 201, "DWORK201");

33245
  ssSetDWorkWidth(S, 201, 1);

33246
  ssSetDWorkDataType(S, 201, SS_SINGLE);

33247
  ssSetDWorkUsedAsDState(S, 201, 1);

33248


33249
  /* '<S151>/Generated S-Function2': DWORK0 */

33250
  ssSetDWorkName(S, 202, "DWORK202");

33251
  ssSetDWorkWidth(S, 202, 1);

33252
  ssSetDWorkDataType(S, 202, SS_SINGLE);

33253
  ssSetDWorkUsedAsDState(S, 202, 1);

33254


33255
  /* '<S151>/Generated S-Function2': DWORK1 */

33256
  ssSetDWorkName(S, 203, "DWORK203");

33257
  ssSetDWorkWidth(S, 203, 1);

33258
  ssSetDWorkDataType(S, 203, SS_SINGLE);

33259
  ssSetDWorkUsedAsDState(S, 203, 1);

33260


33261
  /* '<S149>/Generated S-Function4': DWORK0 */

33262
  ssSetDWorkName(S, 204, "DWORK204");

33263
  ssSetDWorkWidth(S, 204, 1);

33264
  ssSetDWorkDataType(S, 204, SS_SINGLE);

33265
  ssSetDWorkUsedAsDState(S, 204, 1);

33266


33267
  /* '<S149>/Generated S-Function4': DWORK1 */

33268
  ssSetDWorkName(S, 205, "DWORK205");

33269
  ssSetDWorkWidth(S, 205, 1);

33270
  ssSetDWorkDataType(S, 205, SS_SINGLE);

33271
  ssSetDWorkUsedAsDState(S, 205, 1);

33272


33273
  /* '<S142>/Generated S-Function2': DWORK0 */

33274
  ssSetDWorkName(S, 206, "DWORK206");

33275
  ssSetDWorkWidth(S, 206, 1);

33276
  ssSetDWorkDataType(S, 206, SS_SINGLE);

33277
  ssSetDWorkUsedAsDState(S, 206, 1);

33278


33279
  /* '<S142>/Generated S-Function2': DWORK1 */

33280
  ssSetDWorkName(S, 207, "DWORK207");

33281
  ssSetDWorkWidth(S, 207, 1);

33282
  ssSetDWorkDataType(S, 207, SS_SINGLE);

33283
  ssSetDWorkUsedAsDState(S, 207, 1);

33284


33285
  /* '<S140>/Generated S-Function3': DWORK0 */

33286
  ssSetDWorkName(S, 208, "DWORK208");

33287
  ssSetDWorkWidth(S, 208, 1);

33288
  ssSetDWorkDataType(S, 208, SS_SINGLE);

33289
  ssSetDWorkUsedAsDState(S, 208, 1);

33290


33291
  /* '<S140>/Generated S-Function3': DWORK1 */

33292
  ssSetDWorkName(S, 209, "DWORK209");

33293
  ssSetDWorkWidth(S, 209, 1);

33294
  ssSetDWorkDataType(S, 209, SS_SINGLE);

33295
  ssSetDWorkUsedAsDState(S, 209, 1);

33296


33297
  /* '<S141>/Generated S-Function2': DWORK0 */

33298
  ssSetDWorkName(S, 210, "DWORK210");

33299
  ssSetDWorkWidth(S, 210, 1);

33300
  ssSetDWorkDataType(S, 210, SS_SINGLE);

33301
  ssSetDWorkUsedAsDState(S, 210, 1);

33302


33303
  /* '<S141>/Generated S-Function2': DWORK1 */

33304
  ssSetDWorkName(S, 211, "DWORK211");

33305
  ssSetDWorkWidth(S, 211, 1);

33306
  ssSetDWorkDataType(S, 211, SS_SINGLE);

33307
  ssSetDWorkUsedAsDState(S, 211, 1);

33308


33309
  /* '<S139>/Generated S-Function4': DWORK0 */

33310
  ssSetDWorkName(S, 212, "DWORK212");

33311
  ssSetDWorkWidth(S, 212, 1);

33312
  ssSetDWorkDataType(S, 212, SS_SINGLE);

33313
  ssSetDWorkUsedAsDState(S, 212, 1);

33314


33315
  /* '<S139>/Generated S-Function4': DWORK1 */

33316
  ssSetDWorkName(S, 213, "DWORK213");

33317
  ssSetDWorkWidth(S, 213, 1);

33318
  ssSetDWorkDataType(S, 213, SS_SINGLE);

33319
  ssSetDWorkUsedAsDState(S, 213, 1);

33320


33321
  /* '<S160>/Generated S-Function3': DWORK0 */

33322
  ssSetDWorkName(S, 214, "DWORK214");

33323
  ssSetDWorkWidth(S, 214, 1);

33324
  ssSetDWorkDataType(S, 214, SS_SINGLE);

33325
  ssSetDWorkUsedAsDState(S, 214, 1);

33326


33327
  /* '<S160>/Generated S-Function3': DWORK1 */

33328
  ssSetDWorkName(S, 215, "DWORK215");

33329
  ssSetDWorkWidth(S, 215, 1);

33330
  ssSetDWorkDataType(S, 215, SS_SINGLE);

33331
  ssSetDWorkUsedAsDState(S, 215, 1);

33332


33333
  /* '<S159>/Generated S-Function4': DWORK0 */

33334
  ssSetDWorkName(S, 216, "DWORK216");

33335
  ssSetDWorkWidth(S, 216, 1);

33336
  ssSetDWorkDataType(S, 216, SS_SINGLE);

33337
  ssSetDWorkUsedAsDState(S, 216, 1);

33338


33339
  /* '<S159>/Generated S-Function4': DWORK1 */

33340
  ssSetDWorkName(S, 217, "DWORK217");

33341
  ssSetDWorkWidth(S, 217, 1);

33342
  ssSetDWorkDataType(S, 217, SS_SINGLE);

33343
  ssSetDWorkUsedAsDState(S, 217, 1);

33344


33345
  /* '<S195>/Unit Delay1': DSTATE */

33346
  ssSetDWorkName(S, 218, "DWORK218");

33347
  ssSetDWorkWidth(S, 218, 1);

33348
  ssSetDWorkDataType(S, 218, SS_SINGLE);

33349
  ssSetDWorkUsedAsDState(S, 218, 1);

33350


33351
  /* '<S190>/Generated S-Function4': DWORK0 */

33352
  ssSetDWorkName(S, 219, "DWORK219");

33353
  ssSetDWorkWidth(S, 219, 1);

33354
  ssSetDWorkDataType(S, 219, SS_SINGLE);

33355
  ssSetDWorkUsedAsDState(S, 219, 1);

33356


33357
  /* '<S190>/Generated S-Function4': DWORK1 */

33358
  ssSetDWorkName(S, 220, "DWORK220");

33359
  ssSetDWorkWidth(S, 220, 1);

33360
  ssSetDWorkDataType(S, 220, SS_SINGLE);

33361
  ssSetDWorkUsedAsDState(S, 220, 1);

33362


33363
  /* '<S191>/Generated S-Function4': DWORK0 */

33364
  ssSetDWorkName(S, 221, "DWORK221");

33365
  ssSetDWorkWidth(S, 221, 1);

33366
  ssSetDWorkDataType(S, 221, SS_SINGLE);

33367
  ssSetDWorkUsedAsDState(S, 221, 1);

33368


33369
  /* '<S191>/Generated S-Function4': DWORK1 */

33370
  ssSetDWorkName(S, 222, "DWORK222");

33371
  ssSetDWorkWidth(S, 222, 1);

33372
  ssSetDWorkDataType(S, 222, SS_SINGLE);

33373
  ssSetDWorkUsedAsDState(S, 222, 1);

33374


33375
  /* '<S197>/Generated S-Function2': DWORK0 */

33376
  ssSetDWorkName(S, 223, "DWORK223");

33377
  ssSetDWorkWidth(S, 223, 1);

33378
  ssSetDWorkDataType(S, 223, SS_SINGLE);

33379
  ssSetDWorkUsedAsDState(S, 223, 1);

33380


33381
  /* '<S197>/Generated S-Function2': DWORK1 */

33382
  ssSetDWorkName(S, 224, "DWORK224");

33383
  ssSetDWorkWidth(S, 224, 1);

33384
  ssSetDWorkDataType(S, 224, SS_SINGLE);

33385
  ssSetDWorkUsedAsDState(S, 224, 1);

33386


33387
  /* '<S196>/Generated S-Function4': DWORK0 */

33388
  ssSetDWorkName(S, 225, "DWORK225");

33389
  ssSetDWorkWidth(S, 225, 1);

33390
  ssSetDWorkDataType(S, 225, SS_SINGLE);

33391
  ssSetDWorkUsedAsDState(S, 225, 1);

33392


33393
  /* '<S196>/Generated S-Function4': DWORK1 */

33394
  ssSetDWorkName(S, 226, "DWORK226");

33395
  ssSetDWorkWidth(S, 226, 1);

33396
  ssSetDWorkDataType(S, 226, SS_SINGLE);

33397
  ssSetDWorkUsedAsDState(S, 226, 1);

33398


33399
  /* '<S201>/Generated S-Function2': DWORK0 */

33400
  ssSetDWorkName(S, 227, "DWORK227");

33401
  ssSetDWorkWidth(S, 227, 1);

33402
  ssSetDWorkDataType(S, 227, SS_SINGLE);

33403
  ssSetDWorkUsedAsDState(S, 227, 1);

33404


33405
  /* '<S201>/Generated S-Function2': DWORK1 */

33406
  ssSetDWorkName(S, 228, "DWORK228");

33407
  ssSetDWorkWidth(S, 228, 1);

33408
  ssSetDWorkDataType(S, 228, SS_SINGLE);

33409
  ssSetDWorkUsedAsDState(S, 228, 1);

33410


33411
  /* '<S200>/Generated S-Function4': DWORK0 */

33412
  ssSetDWorkName(S, 229, "DWORK229");

33413
  ssSetDWorkWidth(S, 229, 1);

33414
  ssSetDWorkDataType(S, 229, SS_SINGLE);

33415
  ssSetDWorkUsedAsDState(S, 229, 1);

33416


33417
  /* '<S200>/Generated S-Function4': DWORK1 */

33418
  ssSetDWorkName(S, 230, "DWORK230");

33419
  ssSetDWorkWidth(S, 230, 1);

33420
  ssSetDWorkDataType(S, 230, SS_SINGLE);

33421
  ssSetDWorkUsedAsDState(S, 230, 1);

33422


33423
  /* '<S73>/Unit Delay1': DSTATE */

33424
  ssSetDWorkName(S, 231, "DWORK231");

33425
  ssSetDWorkWidth(S, 231, 1);

33426
  ssSetDWorkDataType(S, 231, SS_SINGLE);

33427
  ssSetDWorkUsedAsDState(S, 231, 1);

33428


33429
  /* '<S69>/Generated S-Function4': DWORK0 */

33430
  ssSetDWorkName(S, 232, "DWORK232");

33431
  ssSetDWorkWidth(S, 232, 1);

33432
  ssSetDWorkDataType(S, 232, SS_SINGLE);

33433
  ssSetDWorkUsedAsDState(S, 232, 1);

33434


33435
  /* '<S69>/Generated S-Function4': DWORK1 */

33436
  ssSetDWorkName(S, 233, "DWORK233");

33437
  ssSetDWorkWidth(S, 233, 1);

33438
  ssSetDWorkDataType(S, 233, SS_SINGLE);

33439
  ssSetDWorkUsedAsDState(S, 233, 1);

33440


33441
  /* '<S70>/Generated S-Function4': DWORK0 */

33442
  ssSetDWorkName(S, 234, "DWORK234");

33443
  ssSetDWorkWidth(S, 234, 1);

33444
  ssSetDWorkDataType(S, 234, SS_SINGLE);

33445
  ssSetDWorkUsedAsDState(S, 234, 1);

33446


33447
  /* '<S70>/Generated S-Function4': DWORK1 */

33448
  ssSetDWorkName(S, 235, "DWORK235");

33449
  ssSetDWorkWidth(S, 235, 1);

33450
  ssSetDWorkDataType(S, 235, SS_SINGLE);

33451
  ssSetDWorkUsedAsDState(S, 235, 1);

33452


33453
  /* '<S75>/Generated S-Function2': DWORK0 */

33454
  ssSetDWorkName(S, 236, "DWORK236");

33455
  ssSetDWorkWidth(S, 236, 1);

33456
  ssSetDWorkDataType(S, 236, SS_SINGLE);

33457
  ssSetDWorkUsedAsDState(S, 236, 1);

33458


33459
  /* '<S75>/Generated S-Function2': DWORK1 */

33460
  ssSetDWorkName(S, 237, "DWORK237");

33461
  ssSetDWorkWidth(S, 237, 1);

33462
  ssSetDWorkDataType(S, 237, SS_SINGLE);

33463
  ssSetDWorkUsedAsDState(S, 237, 1);

33464


33465
  /* '<S74>/Generated S-Function4': DWORK0 */

33466
  ssSetDWorkName(S, 238, "DWORK238");

33467
  ssSetDWorkWidth(S, 238, 1);

33468
  ssSetDWorkDataType(S, 238, SS_SINGLE);

33469
  ssSetDWorkUsedAsDState(S, 238, 1);

33470


33471
  /* '<S74>/Generated S-Function4': DWORK1 */

33472
  ssSetDWorkName(S, 239, "DWORK239");

33473
  ssSetDWorkWidth(S, 239, 1);

33474
  ssSetDWorkDataType(S, 239, SS_SINGLE);

33475
  ssSetDWorkUsedAsDState(S, 239, 1);

33476


33477
  /* '<S208>/Generated S-Function': DWORK0 */

33478
  ssSetDWorkName(S, 240, "DWORK240");

33479
  ssSetDWorkWidth(S, 240, 1);

33480
  ssSetDWorkDataType(S, 240, SS_SINGLE);

33481
  ssSetDWorkUsedAsDState(S, 240, 1);

33482


33483
  /* '<S241>/Generated S-Function4': DWORK0 */

33484
  ssSetDWorkName(S, 241, "DWORK241");

33485
  ssSetDWorkWidth(S, 241, 1);

33486
  ssSetDWorkDataType(S, 241, SS_SINGLE);

33487
  ssSetDWorkUsedAsDState(S, 241, 1);

33488


33489
  /* '<S241>/Generated S-Function4': DWORK1 */

33490
  ssSetDWorkName(S, 242, "DWORK242");

33491
  ssSetDWorkWidth(S, 242, 1);

33492
  ssSetDWorkDataType(S, 242, SS_SINGLE);

33493
  ssSetDWorkUsedAsDState(S, 242, 1);

33494


33495
  /* '<S239>/Generated S-Function4': DWORK0 */

33496
  ssSetDWorkName(S, 243, "DWORK243");

33497
  ssSetDWorkWidth(S, 243, 1);

33498
  ssSetDWorkDataType(S, 243, SS_SINGLE);

33499
  ssSetDWorkUsedAsDState(S, 243, 1);

33500


33501
  /* '<S239>/Generated S-Function4': DWORK1 */

33502
  ssSetDWorkName(S, 244, "DWORK244");

33503
  ssSetDWorkWidth(S, 244, 1);

33504
  ssSetDWorkDataType(S, 244, SS_SINGLE);

33505
  ssSetDWorkUsedAsDState(S, 244, 1);

33506


33507
  /* '<S240>/Generated S-Function4': DWORK0 */

33508
  ssSetDWorkName(S, 245, "DWORK245");

33509
  ssSetDWorkWidth(S, 245, 1);

33510
  ssSetDWorkDataType(S, 245, SS_SINGLE);

33511
  ssSetDWorkUsedAsDState(S, 245, 1);

33512


33513
  /* '<S240>/Generated S-Function4': DWORK1 */

33514
  ssSetDWorkName(S, 246, "DWORK246");

33515
  ssSetDWorkWidth(S, 246, 1);

33516
  ssSetDWorkDataType(S, 246, SS_SINGLE);

33517
  ssSetDWorkUsedAsDState(S, 246, 1);

33518


33519
  /* '<S238>/Generated S-Function8': DWORK0 */

33520
  ssSetDWorkName(S, 247, "DWORK247");

33521
  ssSetDWorkWidth(S, 247, 1);

33522
  ssSetDWorkDataType(S, 247, SS_SINGLE);

33523
  ssSetDWorkUsedAsDState(S, 247, 1);

33524


33525
  /* '<S238>/Generated S-Function8': DWORK1 */

33526
  ssSetDWorkName(S, 248, "DWORK248");

33527
  ssSetDWorkWidth(S, 248, 1);

33528
  ssSetDWorkDataType(S, 248, SS_SINGLE);

33529
  ssSetDWorkUsedAsDState(S, 248, 1);

33530


33531
  /* '<S223>/Generated S-Function7': DWORK0 */

33532
  ssSetDWorkName(S, 249, "DWORK249");

33533
  ssSetDWorkWidth(S, 249, 1);

33534
  ssSetDWorkDataType(S, 249, SS_SINGLE);

33535
  ssSetDWorkUsedAsDState(S, 249, 1);

33536


33537
  /* '<S224>/Generated S-Function7': DWORK0 */

33538
  ssSetDWorkName(S, 250, "DWORK250");

33539
  ssSetDWorkWidth(S, 250, 1);

33540
  ssSetDWorkDataType(S, 250, SS_SINGLE);

33541
  ssSetDWorkUsedAsDState(S, 250, 1);

33542


33543
  /* '<S252>/Generated S-Function8': DWORK0 */

33544
  ssSetDWorkName(S, 251, "DWORK251");

33545
  ssSetDWorkWidth(S, 251, 1);

33546
  ssSetDWorkDataType(S, 251, SS_SINGLE);

33547
  ssSetDWorkUsedAsDState(S, 251, 1);

33548


33549
  /* '<S252>/Generated S-Function8': DWORK1 */

33550
  ssSetDWorkName(S, 252, "DWORK252");

33551
  ssSetDWorkWidth(S, 252, 1);

33552
  ssSetDWorkDataType(S, 252, SS_SINGLE);

33553
  ssSetDWorkUsedAsDState(S, 252, 1);

33554


33555
  /* '<S253>/Generated S-Function4': DWORK0 */

33556
  ssSetDWorkName(S, 253, "DWORK253");

33557
  ssSetDWorkWidth(S, 253, 1);

33558
  ssSetDWorkDataType(S, 253, SS_SINGLE);

33559
  ssSetDWorkUsedAsDState(S, 253, 1);

33560


33561
  /* '<S253>/Generated S-Function4': DWORK1 */

33562
  ssSetDWorkName(S, 254, "DWORK254");

33563
  ssSetDWorkWidth(S, 254, 1);

33564
  ssSetDWorkDataType(S, 254, SS_SINGLE);

33565
  ssSetDWorkUsedAsDState(S, 254, 1);

33566


33567
  /* '<S254>/Generated S-Function4': DWORK0 */

33568
  ssSetDWorkName(S, 255, "DWORK255");

33569
  ssSetDWorkWidth(S, 255, 1);

33570
  ssSetDWorkDataType(S, 255, SS_SINGLE);

33571
  ssSetDWorkUsedAsDState(S, 255, 1);

33572


33573
  /* '<S254>/Generated S-Function4': DWORK1 */

33574
  ssSetDWorkName(S, 256, "DWORK256");

33575
  ssSetDWorkWidth(S, 256, 1);

33576
  ssSetDWorkDataType(S, 256, SS_SINGLE);

33577
  ssSetDWorkUsedAsDState(S, 256, 1);

33578


33579
  /* '<S266>/Generated S-Function': DWORK0 */

33580
  ssSetDWorkName(S, 257, "DWORK257");

33581
  ssSetDWorkWidth(S, 257, 1);

33582
  ssSetDWorkDataType(S, 257, SS_SINGLE);

33583
  ssSetDWorkUsedAsDState(S, 257, 1);

33584


33585
  /* '<S266>/Generated S-Function': DWORK1 */

33586
  ssSetDWorkName(S, 258, "DWORK258");

33587
  ssSetDWorkWidth(S, 258, 1);

33588
  ssSetDWorkDataType(S, 258, SS_SINGLE);

33589
  ssSetDWorkUsedAsDState(S, 258, 1);

33590


33591
  /* '<S265>/Generated S-Function8': DWORK0 */

33592
  ssSetDWorkName(S, 259, "DWORK259");

33593
  ssSetDWorkWidth(S, 259, 1);

33594
  ssSetDWorkDataType(S, 259, SS_SINGLE);

33595
  ssSetDWorkUsedAsDState(S, 259, 1);

33596


33597
  /* '<S265>/Generated S-Function8': DWORK1 */

33598
  ssSetDWorkName(S, 260, "DWORK260");

33599
  ssSetDWorkWidth(S, 260, 1);

33600
  ssSetDWorkDataType(S, 260, SS_SINGLE);

33601
  ssSetDWorkUsedAsDState(S, 260, 1);

33602


33603
  /* '<S273>/Unit Delay1': DSTATE */

33604
  ssSetDWorkName(S, 261, "DWORK261");

33605
  ssSetDWorkWidth(S, 261, 1);

33606
  ssSetDWorkDataType(S, 261, SS_SINGLE);

33607
  ssSetDWorkUsedAsDState(S, 261, 1);

33608


33609
  /* '<S247>/Unit Delay1': DSTATE */

33610
  ssSetDWorkName(S, 262, "DWORK262");

33611
  ssSetDWorkWidth(S, 262, 1);

33612
  ssSetDWorkDataType(S, 262, SS_SINGLE);

33613
  ssSetDWorkUsedAsDState(S, 262, 1);

33614


33615
  /* '<S257>/Generated S-Function3': DWORK0 */

33616
  ssSetDWorkName(S, 263, "DWORK263");

33617
  ssSetDWorkWidth(S, 263, 1);

33618
  ssSetDWorkDataType(S, 263, SS_SINGLE);

33619
  ssSetDWorkUsedAsDState(S, 263, 1);

33620


33621
  /* '<S257>/Generated S-Function3': DWORK1 */

33622
  ssSetDWorkName(S, 264, "DWORK264");

33623
  ssSetDWorkWidth(S, 264, 1);

33624
  ssSetDWorkDataType(S, 264, SS_SINGLE);

33625
  ssSetDWorkUsedAsDState(S, 264, 1);

33626


33627
  /* '<S249>/Generated S-Function7': DWORK0 */

33628
  ssSetDWorkName(S, 265, "DWORK265");

33629
  ssSetDWorkWidth(S, 265, 1);

33630
  ssSetDWorkDataType(S, 265, SS_SINGLE);

33631
  ssSetDWorkUsedAsDState(S, 265, 1);

33632


33633
  /* '<S274>/Generated S-Function7': DWORK0 */

33634
  ssSetDWorkName(S, 266, "DWORK266");

33635
  ssSetDWorkWidth(S, 266, 1);

33636
  ssSetDWorkDataType(S, 266, SS_SINGLE);

33637
  ssSetDWorkUsedAsDState(S, 266, 1);

33638


33639
  /* '<S215>/Generated S-Function4': DWORK0 */

33640
  ssSetDWorkName(S, 267, "DWORK267");

33641
  ssSetDWorkWidth(S, 267, 1);

33642
  ssSetDWorkDataType(S, 267, SS_SINGLE);

33643
  ssSetDWorkUsedAsDState(S, 267, 1);

33644


33645
  /* '<S215>/Generated S-Function4': DWORK1 */

33646
  ssSetDWorkName(S, 268, "DWORK268");

33647
  ssSetDWorkWidth(S, 268, 1);

33648
  ssSetDWorkDataType(S, 268, SS_SINGLE);

33649
  ssSetDWorkUsedAsDState(S, 268, 1);

33650


33651
  /* '<S216>/Generated S-Function7': DWORK0 */

33652
  ssSetDWorkName(S, 269, "DWORK269");

33653
  ssSetDWorkWidth(S, 269, 1);

33654
  ssSetDWorkDataType(S, 269, SS_SINGLE);

33655
  ssSetDWorkUsedAsDState(S, 269, 1);

33656


33657
  /* '<S433>/Memory10': PreviousInput */

33658
  ssSetDWorkName(S, 270, "DWORK270");

33659
  ssSetDWorkWidth(S, 270, 1);

33660
  ssSetDWorkDataType(S, 270, SS_SINGLE);

33661


33662
  /* '<S438>/Memory1': PreviousInput */

33663
  ssSetDWorkName(S, 271, "DWORK271");

33664
  ssSetDWorkWidth(S, 271, 1);

33665
  ssSetDWorkDataType(S, 271, SS_SINGLE);

33666


33667
  /* '<S437>/Memory1': PreviousInput */

33668
  ssSetDWorkName(S, 272, "DWORK272");

33669
  ssSetDWorkWidth(S, 272, 1);

33670
  ssSetDWorkDataType(S, 272, SS_SINGLE);

33671


33672
  /* '<S436>/Memory1': PreviousInput */

33673
  ssSetDWorkName(S, 273, "DWORK273");

33674
  ssSetDWorkWidth(S, 273, 1);

33675
  ssSetDWorkDataType(S, 273, SS_SINGLE);

33676


33677
  /* '<S435>/Memory1': PreviousInput */

33678
  ssSetDWorkName(S, 274, "DWORK274");

33679
  ssSetDWorkWidth(S, 274, 1);

33680
  ssSetDWorkDataType(S, 274, SS_SINGLE);

33681


33682
  /* '<S434>/Memory1': PreviousInput */

33683
  ssSetDWorkName(S, 275, "DWORK275");

33684
  ssSetDWorkWidth(S, 275, 1);

33685
  ssSetDWorkDataType(S, 275, SS_SINGLE);

33686


33687
  /* '<S439>/Memory1': PreviousInput */

33688
  ssSetDWorkName(S, 276, "DWORK276");

33689
  ssSetDWorkWidth(S, 276, 1);

33690
  ssSetDWorkDataType(S, 276, SS_SINGLE);

33691


33692
  /* '<S433>/Memory1': PreviousInput */

33693
  ssSetDWorkName(S, 277, "DWORK277");

33694
  ssSetDWorkWidth(S, 277, 1);

33695
  ssSetDWorkDataType(S, 277, SS_SINGLE);

33696


33697
  /* '<S433>/Memory2': PreviousInput */

33698
  ssSetDWorkName(S, 278, "DWORK278");

33699
  ssSetDWorkWidth(S, 278, 1);

33700
  ssSetDWorkDataType(S, 278, SS_SINGLE);

33701


33702
  /* '<S433>/Memory3': PreviousInput */

33703
  ssSetDWorkName(S, 279, "DWORK279");

33704
  ssSetDWorkWidth(S, 279, 1);

33705
  ssSetDWorkDataType(S, 279, SS_SINGLE);

33706


33707
  /* '<S433>/Memory4': PreviousInput */

33708
  ssSetDWorkName(S, 280, "DWORK280");

33709
  ssSetDWorkWidth(S, 280, 1);

33710
  ssSetDWorkDataType(S, 280, SS_SINGLE);

33711


33712
  /* '<S433>/Memory5': PreviousInput */

33713
  ssSetDWorkName(S, 281, "DWORK281");

33714
  ssSetDWorkWidth(S, 281, 1);

33715
  ssSetDWorkDataType(S, 281, SS_SINGLE);

33716


33717
  /* '<S433>/Memory6': PreviousInput */

33718
  ssSetDWorkName(S, 282, "DWORK282");

33719
  ssSetDWorkWidth(S, 282, 1);

33720
  ssSetDWorkDataType(S, 282, SS_SINGLE);

33721


33722
  /* '<S433>/Memory7': PreviousInput */

33723
  ssSetDWorkName(S, 283, "DWORK283");

33724
  ssSetDWorkWidth(S, 283, 1);

33725
  ssSetDWorkDataType(S, 283, SS_SINGLE);

33726


33727
  /* '<S433>/Memory8': PreviousInput */

33728
  ssSetDWorkName(S, 284, "DWORK284");

33729
  ssSetDWorkWidth(S, 284, 1);

33730
  ssSetDWorkDataType(S, 284, SS_SINGLE);

33731


33732
  /* '<S433>/Memory9': PreviousInput */

33733
  ssSetDWorkName(S, 285, "DWORK285");

33734
  ssSetDWorkWidth(S, 285, 1);

33735
  ssSetDWorkDataType(S, 285, SS_SINGLE);

33736


33737
  /* '<S316>/Landing2': DWORK21 */

33738
  ssSetDWorkName(S, 286, "DWORK286");

33739
  ssSetDWorkWidth(S, 286, 1);

33740
  ssSetDWorkDataType(S, 286, SS_SINGLE);

33741


33742
  /* '<S293>/CompTP': DWORK1 */

33743
  ssSetDWorkName(S, 287, "DWORK287");

33744
  ssSetDWorkWidth(S, 287, 1);

33745
  ssSetDWorkDataType(S, 287, SS_SINGLE);

33746


33747
  /* '<S293>/CompTP': DWORK2 */

33748
  ssSetDWorkName(S, 288, "DWORK288");

33749
  ssSetDWorkWidth(S, 288, 1);

33750
  ssSetDWorkDataType(S, 288, SS_SINGLE);

33751


33752
  /* '<S81>/Generated S-Function5': DWORK0 */

33753
  ssSetDWorkName(S, 289, "DWORK289");

33754
  ssSetDWorkWidth(S, 289, 1);

33755
  ssSetDWorkDataType(S, 289, SS_SINGLE);

33756


33757
  /* '<S81>/Generated S-Function5': DWORK1 */

33758
  ssSetDWorkName(S, 290, "DWORK290");

33759
  ssSetDWorkWidth(S, 290, 1);

33760
  ssSetDWorkDataType(S, 290, SS_SINGLE);

33761


33762
  /* '<S81>/Generated S-Function5': DWORK2 */

33763
  ssSetDWorkName(S, 291, "DWORK291");

33764
  ssSetDWorkWidth(S, 291, 1);

33765
  ssSetDWorkDataType(S, 291, SS_SINGLE);

33766


33767
  /* '<S123>/Generated S-Function5': DWORK0 */

33768
  ssSetDWorkName(S, 292, "DWORK292");

33769
  ssSetDWorkWidth(S, 292, 1);

33770
  ssSetDWorkDataType(S, 292, SS_SINGLE);

33771


33772
  /* '<S123>/Generated S-Function5': DWORK1 */

33773
  ssSetDWorkName(S, 293, "DWORK293");

33774
  ssSetDWorkWidth(S, 293, 1);

33775
  ssSetDWorkDataType(S, 293, SS_SINGLE);

33776


33777
  /* '<S123>/Generated S-Function5': DWORK2 */

33778
  ssSetDWorkName(S, 294, "DWORK294");

33779
  ssSetDWorkWidth(S, 294, 1);

33780
  ssSetDWorkDataType(S, 294, SS_SINGLE);

33781


33782
  /* '<S12>/LLACalibration2': DWORK4 */

33783
  ssSetDWorkName(S, 295, "DWORK295");

33784
  ssSetDWorkWidth(S, 295, 1);

33785
  ssSetDWorkDataType(S, 295, SS_INT32);

33786


33787
  /* '<S32>/wowcmp1': DWORK4 */

33788
  ssSetDWorkName(S, 296, "DWORK296");

33789
  ssSetDWorkWidth(S, 296, 1);

33790
  ssSetDWorkDataType(S, 296, SS_INT32);

33791


33792
  /* '<S322>/Taxi': DWORK6 */

33793
  ssSetDWorkName(S, 297, "DWORK297");

33794
  ssSetDWorkWidth(S, 297, 1);

33795
  ssSetDWorkDataType(S, 297, SS_INT32);

33796


33797
  /* '<S322>/Taxi': DWORK7 */

33798
  ssSetDWorkName(S, 298, "DWORK298");

33799
  ssSetDWorkWidth(S, 298, 1);

33800
  ssSetDWorkDataType(S, 298, SS_INT32);

33801


33802
  /* '<S322>/Taxi': DWORK8 */

33803
  ssSetDWorkName(S, 299, "DWORK299");

33804
  ssSetDWorkWidth(S, 299, 1);

33805
  ssSetDWorkDataType(S, 299, SS_INT32);

33806


33807
  /* '<S322>/Taxi': DWORK9 */

33808
  ssSetDWorkName(S, 300, "DWORK300");

33809
  ssSetDWorkWidth(S, 300, 1);

33810
  ssSetDWorkDataType(S, 300, SS_INT32);

33811


33812
  /* '<S316>/Landing2': DWORK16 */

33813
  ssSetDWorkName(S, 301, "DWORK301");

33814
  ssSetDWorkWidth(S, 301, 1);

33815
  ssSetDWorkDataType(S, 301, SS_INT32);

33816


33817
  /* '<S316>/Landing2': DWORK17 */

33818
  ssSetDWorkName(S, 302, "DWORK302");

33819
  ssSetDWorkWidth(S, 302, 1);

33820
  ssSetDWorkDataType(S, 302, SS_INT32);

33821


33822
  /* '<S316>/Landing2': DWORK18 */

33823
  ssSetDWorkName(S, 303, "DWORK303");

33824
  ssSetDWorkWidth(S, 303, 1);

33825
  ssSetDWorkDataType(S, 303, SS_INT32);

33826


33827
  /* '<S316>/Landing2': DWORK19 */

33828
  ssSetDWorkName(S, 304, "DWORK304");

33829
  ssSetDWorkWidth(S, 304, 1);

33830
  ssSetDWorkDataType(S, 304, SS_INT32);

33831


33832
  /* '<S316>/Landing2': DWORK20 */

33833
  ssSetDWorkName(S, 305, "DWORK305");

33834
  ssSetDWorkWidth(S, 305, 1);

33835
  ssSetDWorkDataType(S, 305, SS_INT32);

33836


33837
  /* '<S316>/Landing2': DWORK22 */

33838
  ssSetDWorkName(S, 306, "DWORK306");

33839
  ssSetDWorkWidth(S, 306, 1);

33840
  ssSetDWorkDataType(S, 306, SS_INT32);

33841


33842
  /* '<S316>/Landing2': DWORK23 */

33843
  ssSetDWorkName(S, 307, "DWORK307");

33844
  ssSetDWorkWidth(S, 307, 1);

33845
  ssSetDWorkDataType(S, 307, SS_INT32);

33846


33847
  /* '<S316>/Landing2': DWORK24 */

33848
  ssSetDWorkName(S, 308, "DWORK308");

33849
  ssSetDWorkWidth(S, 308, 1);

33850
  ssSetDWorkDataType(S, 308, SS_INT32);

33851


33852
  /* '<S316>/Landing2': DWORK26 */

33853
  ssSetDWorkName(S, 309, "DWORK309");

33854
  ssSetDWorkWidth(S, 309, 1);

33855
  ssSetDWorkDataType(S, 309, SS_INT32);

33856


33857
  /* '<S316>/Landing2': DWORK27 */

33858
  ssSetDWorkName(S, 310, "DWORK310");

33859
  ssSetDWorkWidth(S, 310, 1);

33860
  ssSetDWorkDataType(S, 310, SS_INT32);

33861


33862
  /* '<S316>/Landing2': DWORK28 */

33863
  ssSetDWorkName(S, 311, "DWORK311");

33864
  ssSetDWorkWidth(S, 311, 1);

33865
  ssSetDWorkDataType(S, 311, SS_INT32);

33866


33867
  /* '<S316>/Landing2': DWORK29 */

33868
  ssSetDWorkName(S, 312, "DWORK312");

33869
  ssSetDWorkWidth(S, 312, 1);

33870
  ssSetDWorkDataType(S, 312, SS_INT32);

33871


33872
  /* '<S313>/Approach2': DWORK11 */

33873
  ssSetDWorkName(S, 313, "DWORK313");

33874
  ssSetDWorkWidth(S, 313, 1);

33875
  ssSetDWorkDataType(S, 313, SS_INT32);

33876


33877
  /* '<S313>/Approach2': DWORK12 */

33878
  ssSetDWorkName(S, 314, "DWORK314");

33879
  ssSetDWorkWidth(S, 314, 1);

33880
  ssSetDWorkDataType(S, 314, SS_INT32);

33881


33882
  /* '<S313>/Approach2': DWORK13 */

33883
  ssSetDWorkName(S, 315, "DWORK315");

33884
  ssSetDWorkWidth(S, 315, 1);

33885
  ssSetDWorkDataType(S, 315, SS_INT32);

33886


33887
  /* '<S313>/Approach2': DWORK14 */

33888
  ssSetDWorkName(S, 316, "DWORK316");

33889
  ssSetDWorkWidth(S, 316, 1);

33890
  ssSetDWorkDataType(S, 316, SS_INT32);

33891


33892
  /* '<S313>/Approach2': DWORK16 */

33893
  ssSetDWorkName(S, 317, "DWORK317");

33894
  ssSetDWorkWidth(S, 317, 1);

33895
  ssSetDWorkDataType(S, 317, SS_INT32);

33896


33897
  /* '<S313>/Approach2': DWORK17 */

33898
  ssSetDWorkName(S, 318, "DWORK318");

33899
  ssSetDWorkWidth(S, 318, 1);

33900
  ssSetDWorkDataType(S, 318, SS_INT32);

33901


33902
  /* '<S315>/Cruise': DWORK11 */

33903
  ssSetDWorkName(S, 319, "DWORK319");

33904
  ssSetDWorkWidth(S, 319, 1);

33905
  ssSetDWorkDataType(S, 319, SS_INT32);

33906


33907
  /* '<S314>/Climb2': DWORK10 */

33908
  ssSetDWorkName(S, 320, "DWORK320");

33909
  ssSetDWorkWidth(S, 320, 1);

33910
  ssSetDWorkDataType(S, 320, SS_INT32);

33911


33912
  /* '<S314>/Climb2': DWORK11 */

33913
  ssSetDWorkName(S, 321, "DWORK321");

33914
  ssSetDWorkWidth(S, 321, 1);

33915
  ssSetDWorkDataType(S, 321, SS_INT32);

33916


33917
  /* '<S321>/Takeoff': DWORK5 */

33918
  ssSetDWorkName(S, 322, "DWORK322");

33919
  ssSetDWorkWidth(S, 322, 1);

33920
  ssSetDWorkDataType(S, 322, SS_INT32);

33921


33922
  /* '<S321>/Takeoff': DWORK6 */

33923
  ssSetDWorkName(S, 323, "DWORK323");

33924
  ssSetDWorkWidth(S, 323, 1);

33925
  ssSetDWorkDataType(S, 323, SS_INT32);

33926


33927
  /* '<S321>/Takeoff': DWORK7 */

33928
  ssSetDWorkName(S, 324, "DWORK324");

33929
  ssSetDWorkWidth(S, 324, 1);

33930
  ssSetDWorkDataType(S, 324, SS_INT32);

33931


33932
  /* '<S321>/Takeoff': DWORK8 */

33933
  ssSetDWorkName(S, 325, "DWORK325");

33934
  ssSetDWorkWidth(S, 325, 1);

33935
  ssSetDWorkDataType(S, 325, SS_INT32);

33936


33937
  /* '<S321>/Takeoff': DWORK9 */

33938
  ssSetDWorkName(S, 326, "DWORK326");

33939
  ssSetDWorkWidth(S, 326, 1);

33940
  ssSetDWorkDataType(S, 326, SS_INT32);

33941


33942
  /* '<S321>/Takeoff': DWORK10 */

33943
  ssSetDWorkName(S, 327, "DWORK327");

33944
  ssSetDWorkWidth(S, 327, 1);

33945
  ssSetDWorkDataType(S, 327, SS_INT32);

33946


33947
  /* '<S319>/Pre_Takeoff': DWORK4 */

33948
  ssSetDWorkName(S, 328, "DWORK328");

33949
  ssSetDWorkWidth(S, 328, 1);

33950
  ssSetDWorkDataType(S, 328, SS_INT32);

33951


33952
  /* '<S319>/Pre_Takeoff': DWORK5 */

33953
  ssSetDWorkName(S, 329, "DWORK329");

33954
  ssSetDWorkWidth(S, 329, 1);

33955
  ssSetDWorkDataType(S, 329, SS_INT32);

33956


33957
  /* '<S319>/Pre_Takeoff': DWORK6 */

33958
  ssSetDWorkName(S, 330, "DWORK330");

33959
  ssSetDWorkWidth(S, 330, 1);

33960
  ssSetDWorkDataType(S, 330, SS_INT32);

33961


33962
  /* '<S29>/LLACalibration2': DWORK4 */

33963
  ssSetDWorkName(S, 331, "DWORK331");

33964
  ssSetDWorkWidth(S, 331, 1);

33965
  ssSetDWorkDataType(S, 331, SS_INT32);

33966


33967
  /* '<S81>/Generated S-Function5': DWORK3 */

33968
  ssSetDWorkName(S, 332, "DWORK332");

33969
  ssSetDWorkWidth(S, 332, 1);

33970
  ssSetDWorkDataType(S, 332, SS_INT32);

33971


33972
  /* '<S81>/Generated S-Function5': DWORK4 */

33973
  ssSetDWorkName(S, 333, "DWORK333");

33974
  ssSetDWorkWidth(S, 333, 1);

33975
  ssSetDWorkDataType(S, 333, SS_INT32);

33976


33977
  /* '<S123>/Generated S-Function5': DWORK3 */

33978
  ssSetDWorkName(S, 334, "DWORK334");

33979
  ssSetDWorkWidth(S, 334, 1);

33980
  ssSetDWorkDataType(S, 334, SS_INT32);

33981


33982
  /* '<S123>/Generated S-Function5': DWORK4 */

33983
  ssSetDWorkName(S, 335, "DWORK335");

33984
  ssSetDWorkWidth(S, 335, 1);

33985
  ssSetDWorkDataType(S, 335, SS_INT32);

33986


33987
  /* '<S21>/delayblock7': DWORK4 */

33988
  ssSetDWorkName(S, 336, "DWORK336");

33989
  ssSetDWorkWidth(S, 336, 1);

33990
  ssSetDWorkDataType(S, 336, SS_INT32);

33991


33992
  /* '<S21>/delayblock6': DWORK4 */

33993
  ssSetDWorkName(S, 337, "DWORK337");

33994
  ssSetDWorkWidth(S, 337, 1);

33995
  ssSetDWorkDataType(S, 337, SS_INT32);

33996


33997
  /* '<S21>/delayblock5': DWORK4 */

33998
  ssSetDWorkName(S, 338, "DWORK338");

33999
  ssSetDWorkWidth(S, 338, 1);

34000
  ssSetDWorkDataType(S, 338, SS_INT32);

34001


34002
  /* '<S21>/delayblock4': DWORK4 */

34003
  ssSetDWorkName(S, 339, "DWORK339");

34004
  ssSetDWorkWidth(S, 339, 1);

34005
  ssSetDWorkDataType(S, 339, SS_INT32);

34006


34007
  /* '<S21>/delayblock3': DWORK4 */

34008
  ssSetDWorkName(S, 340, "DWORK340");

34009
  ssSetDWorkWidth(S, 340, 1);

34010
  ssSetDWorkDataType(S, 340, SS_INT32);

34011


34012
  /* '<S21>/delayblock2': DWORK4 */

34013
  ssSetDWorkName(S, 341, "DWORK341");

34014
  ssSetDWorkWidth(S, 341, 1);

34015
  ssSetDWorkDataType(S, 341, SS_INT32);

34016


34017
  /* '<S21>/delayblock1': DWORK4 */

34018
  ssSetDWorkName(S, 342, "DWORK342");

34019
  ssSetDWorkWidth(S, 342, 1);

34020
  ssSetDWorkDataType(S, 342, SS_INT32);

34021


34022
  /* '<S335>/Data Store Memory1': dsmMem */

34023
  ssSetDWorkName(S, 343, "DWORK343");

34024
  ssSetDWorkWidth(S, 343, 1);

34025
  ssSetDWorkDataType(S, 343, SS_UINT32);

34026


34027
  /* '<S779>/q': IWORK */

34028
  ssSetDWorkName(S, 344, "DWORK344");

34029
  ssSetDWorkWidth(S, 344, 1);

34030
  ssSetDWorkDataType(S, 344, SS_INTEGER);

34031


34032
  /* '<S778>/p1': IWORK */

34033
  ssSetDWorkName(S, 345, "DWORK345");

34034
  ssSetDWorkWidth(S, 345, 1);

34035
  ssSetDWorkDataType(S, 345, SS_INTEGER);

34036


34037
  /* '<S780>/ub,vb,wb': IWORK */

34038
  ssSetDWorkName(S, 346, "DWORK346");

34039
  ssSetDWorkWidth(S, 346, 1);

34040
  ssSetDWorkDataType(S, 346, SS_INTEGER);

34041


34042
  /* '<S470>/p,q,r ': IWORK */

34043
  ssSetDWorkName(S, 347, "DWORK347");

34044
  ssSetDWorkWidth(S, 347, 1);

34045
  ssSetDWorkDataType(S, 347, SS_INTEGER);

34046


34047
  /* '<S683>/Integrator': IWORK */

34048
  ssSetDWorkName(S, 348, "DWORK348");

34049
  ssSetDWorkWidth(S, 348, 1);

34050
  ssSetDWorkDataType(S, 348, SS_INTEGER);

34051


34052
  /* '<S682>/Integrator': IWORK */

34053
  ssSetDWorkName(S, 349, "DWORK349");

34054
  ssSetDWorkWidth(S, 349, 1);

34055
  ssSetDWorkDataType(S, 349, SS_INTEGER);

34056


34057
  /* '<S778>/p': IWORK */

34058
  ssSetDWorkName(S, 350, "DWORK350");

34059
  ssSetDWorkWidth(S, 350, 1);

34060
  ssSetDWorkDataType(S, 350, SS_INTEGER);

34061


34062
  /* '<S335>/Data Store Memory': dsmMem */

34063
  ssSetDWorkName(S, 351, "DWORK351");

34064
  ssSetDWorkWidth(S, 351, 1);

34065
  ssSetDWorkDataType(S, 351, SS_UINT16);

34066


34067
  /* '<S24>/Unit Delay3': DSTATE */

34068
  ssSetDWorkName(S, 352, "DWORK352");

34069
  ssSetDWorkWidth(S, 352, 1);

34070
  ssSetDWorkDataType(S, 352, SS_INT8);

34071
  ssSetDWorkUsedAsDState(S, 352, 1);

34072


34073
  /* '<S7>/Unit Delay': DSTATE */

34074
  ssSetDWorkName(S, 353, "DWORK353");

34075
  ssSetDWorkWidth(S, 353, 1);

34076
  ssSetDWorkDataType(S, 353, SS_INT8);

34077
  ssSetDWorkUsedAsDState(S, 353, 1);

34078


34079
  /* '<S7>/Unit Delay3': DSTATE */

34080
  ssSetDWorkName(S, 354, "DWORK354");

34081
  ssSetDWorkWidth(S, 354, 1);

34082
  ssSetDWorkDataType(S, 354, SS_INT8);

34083
  ssSetDWorkUsedAsDState(S, 354, 1);

34084


34085
  /* '<S9>/Unit Delay': DSTATE */

34086
  ssSetDWorkName(S, 355, "DWORK355");

34087
  ssSetDWorkWidth(S, 355, 1);

34088
  ssSetDWorkDataType(S, 355, SS_INT8);

34089
  ssSetDWorkUsedAsDState(S, 355, 1);

34090


34091
  /* '<S1>/Unit Delay1': DSTATE */

34092
  ssSetDWorkName(S, 356, "DWORK356");

34093
  ssSetDWorkWidth(S, 356, 1);

34094
  ssSetDWorkDataType(S, 356, SS_INT8);

34095
  ssSetDWorkUsedAsDState(S, 356, 1);

34096


34097
  /* '<S31>/Unit Delay': DSTATE */

34098
  ssSetDWorkName(S, 357, "DWORK357");

34099
  ssSetDWorkWidth(S, 357, 1);

34100
  ssSetDWorkDataType(S, 357, SS_INT8);

34101
  ssSetDWorkUsedAsDState(S, 357, 1);

34102


34103
  /* '<S31>/Unit Delay1': DSTATE */

34104
  ssSetDWorkName(S, 358, "DWORK358");

34105
  ssSetDWorkWidth(S, 358, 1);

34106
  ssSetDWorkDataType(S, 358, SS_INT8);

34107
  ssSetDWorkUsedAsDState(S, 358, 1);

34108


34109
  /* '<S31>/Unit Delay2': DSTATE */

34110
  ssSetDWorkName(S, 359, "DWORK359");

34111
  ssSetDWorkWidth(S, 359, 1);

34112
  ssSetDWorkDataType(S, 359, SS_INT8);

34113
  ssSetDWorkUsedAsDState(S, 359, 1);

34114


34115
  /* '<S31>/Unit Delay3': DSTATE */

34116
  ssSetDWorkName(S, 360, "DWORK360");

34117
  ssSetDWorkWidth(S, 360, 1);

34118
  ssSetDWorkDataType(S, 360, SS_INT8);

34119
  ssSetDWorkUsedAsDState(S, 360, 1);

34120


34121
  /* '<S31>/Unit Delay4': DSTATE */

34122
  ssSetDWorkName(S, 361, "DWORK361");

34123
  ssSetDWorkWidth(S, 361, 1);

34124
  ssSetDWorkDataType(S, 361, SS_INT8);

34125
  ssSetDWorkUsedAsDState(S, 361, 1);

34126


34127
  /* '<S31>/Unit Delay7': DSTATE */

34128
  ssSetDWorkName(S, 362, "DWORK362");

34129
  ssSetDWorkWidth(S, 362, 1);

34130
  ssSetDWorkDataType(S, 362, SS_INT8);

34131
  ssSetDWorkUsedAsDState(S, 362, 1);

34132


34133
  /* '<S31>/Unit Delay5': DSTATE */

34134
  ssSetDWorkName(S, 363, "DWORK363");

34135
  ssSetDWorkWidth(S, 363, 1);

34136
  ssSetDWorkDataType(S, 363, SS_INT8);

34137
  ssSetDWorkUsedAsDState(S, 363, 1);

34138


34139
  /* '<S32>/Unit Delay3': DSTATE */

34140
  ssSetDWorkName(S, 364, "DWORK364");

34141
  ssSetDWorkWidth(S, 364, 1);

34142
  ssSetDWorkDataType(S, 364, SS_INT8);

34143
  ssSetDWorkUsedAsDState(S, 364, 1);

34144


34145
  /* '<S309>/Unit Delay6': DSTATE */

34146
  ssSetDWorkName(S, 365, "DWORK365");

34147
  ssSetDWorkWidth(S, 365, 1);

34148
  ssSetDWorkDataType(S, 365, SS_INT8);

34149
  ssSetDWorkUsedAsDState(S, 365, 1);

34150


34151
  /* '<S309>/Unit Delay5': DSTATE */

34152
  ssSetDWorkName(S, 366, "DWORK366");

34153
  ssSetDWorkWidth(S, 366, 1);

34154
  ssSetDWorkDataType(S, 366, SS_INT8);

34155
  ssSetDWorkUsedAsDState(S, 366, 1);

34156


34157
  /* '<S1>/Unit Delay': DSTATE */

34158
  ssSetDWorkName(S, 367, "DWORK367");

34159
  ssSetDWorkWidth(S, 367, 1);

34160
  ssSetDWorkDataType(S, 367, SS_INT8);

34161
  ssSetDWorkUsedAsDState(S, 367, 1);

34162


34163
  /* '<S430>/Unit Delay6': DSTATE */

34164
  ssSetDWorkName(S, 368, "DWORK368");

34165
  ssSetDWorkWidth(S, 368, 1);

34166
  ssSetDWorkDataType(S, 368, SS_INT8);

34167
  ssSetDWorkUsedAsDState(S, 368, 1);

34168


34169
  /* '<S336>/Unit Delay2': DSTATE */

34170
  ssSetDWorkName(S, 369, "DWORK369");

34171
  ssSetDWorkWidth(S, 369, 1);

34172
  ssSetDWorkDataType(S, 369, SS_INT8);

34173
  ssSetDWorkUsedAsDState(S, 369, 1);

34174


34175
  /* '<S430>/Unit Delay5': DSTATE */

34176
  ssSetDWorkName(S, 370, "DWORK370");

34177
  ssSetDWorkWidth(S, 370, 1);

34178
  ssSetDWorkDataType(S, 370, SS_INT8);

34179
  ssSetDWorkUsedAsDState(S, 370, 1);

34180


34181
  /* '<S682>/Unit Delay': DSTATE */

34182
  ssSetDWorkName(S, 371, "DWORK371");

34183
  ssSetDWorkWidth(S, 371, 1);

34184
  ssSetDWorkDataType(S, 371, SS_INT8);

34185
  ssSetDWorkUsedAsDState(S, 371, 1);

34186


34187
  /* '<S683>/Unit Delay': DSTATE */

34188
  ssSetDWorkName(S, 372, "DWORK372");

34189
  ssSetDWorkWidth(S, 372, 1);

34190
  ssSetDWorkDataType(S, 372, SS_INT8);

34191
  ssSetDWorkUsedAsDState(S, 372, 1);

34192


34193
  /* '<S175>/Unit Delay': DSTATE */

34194
  ssSetDWorkName(S, 373, "DWORK373");

34195
  ssSetDWorkWidth(S, 373, 1);

34196
  ssSetDWorkDataType(S, 373, SS_INT8);

34197
  ssSetDWorkUsedAsDState(S, 373, 1);

34198


34199
  /* '<S174>/Generated S-Function': DWORK2 */

34200
  ssSetDWorkName(S, 374, "DWORK374");

34201
  ssSetDWorkWidth(S, 374, 1);

34202
  ssSetDWorkDataType(S, 374, SS_INT8);

34203
  ssSetDWorkUsedAsDState(S, 374, 1);

34204


34205
  /* '<S174>/Generated S-Function': DWORK3 */

34206
  ssSetDWorkName(S, 375, "DWORK375");

34207
  ssSetDWorkWidth(S, 375, 1);

34208
  ssSetDWorkDataType(S, 375, SS_INT8);

34209
  ssSetDWorkUsedAsDState(S, 375, 1);

34210


34211
  /* '<S185>/Generated S-Function': DWORK2 */

34212
  ssSetDWorkName(S, 376, "DWORK376");

34213
  ssSetDWorkWidth(S, 376, 1);

34214
  ssSetDWorkDataType(S, 376, SS_INT8);

34215
  ssSetDWorkUsedAsDState(S, 376, 1);

34216


34217
  /* '<S185>/Generated S-Function': DWORK3 */

34218
  ssSetDWorkName(S, 377, "DWORK377");

34219
  ssSetDWorkWidth(S, 377, 1);

34220
  ssSetDWorkDataType(S, 377, SS_INT8);

34221
  ssSetDWorkUsedAsDState(S, 377, 1);

34222


34223
  /* '<S184>/Generated S-Function': DWORK2 */

34224
  ssSetDWorkName(S, 378, "DWORK378");

34225
  ssSetDWorkWidth(S, 378, 1);

34226
  ssSetDWorkDataType(S, 378, SS_INT8);

34227
  ssSetDWorkUsedAsDState(S, 378, 1);

34228


34229
  /* '<S184>/Generated S-Function': DWORK3 */

34230
  ssSetDWorkName(S, 379, "DWORK379");

34231
  ssSetDWorkWidth(S, 379, 1);

34232
  ssSetDWorkDataType(S, 379, SS_INT8);

34233
  ssSetDWorkUsedAsDState(S, 379, 1);

34234


34235
  /* '<S183>/Generated S-Function': DWORK2 */

34236
  ssSetDWorkName(S, 380, "DWORK380");

34237
  ssSetDWorkWidth(S, 380, 1);

34238
  ssSetDWorkDataType(S, 380, SS_INT8);

34239
  ssSetDWorkUsedAsDState(S, 380, 1);

34240


34241
  /* '<S183>/Generated S-Function': DWORK3 */

34242
  ssSetDWorkName(S, 381, "DWORK381");

34243
  ssSetDWorkWidth(S, 381, 1);

34244
  ssSetDWorkDataType(S, 381, SS_INT8);

34245
  ssSetDWorkUsedAsDState(S, 381, 1);

34246


34247
  /* '<S178>/Generated S-Function': DWORK2 */

34248
  ssSetDWorkName(S, 382, "DWORK382");

34249
  ssSetDWorkWidth(S, 382, 1);

34250
  ssSetDWorkDataType(S, 382, SS_INT8);

34251
  ssSetDWorkUsedAsDState(S, 382, 1);

34252


34253
  /* '<S178>/Generated S-Function': DWORK3 */

34254
  ssSetDWorkName(S, 383, "DWORK383");

34255
  ssSetDWorkWidth(S, 383, 1);

34256
  ssSetDWorkDataType(S, 383, SS_INT8);

34257
  ssSetDWorkUsedAsDState(S, 383, 1);

34258


34259
  /* '<S177>/Generated S-Function': DWORK2 */

34260
  ssSetDWorkName(S, 384, "DWORK384");

34261
  ssSetDWorkWidth(S, 384, 1);

34262
  ssSetDWorkDataType(S, 384, SS_INT8);

34263
  ssSetDWorkUsedAsDState(S, 384, 1);

34264


34265
  /* '<S177>/Generated S-Function': DWORK3 */

34266
  ssSetDWorkName(S, 385, "DWORK385");

34267
  ssSetDWorkWidth(S, 385, 1);

34268
  ssSetDWorkDataType(S, 385, SS_INT8);

34269
  ssSetDWorkUsedAsDState(S, 385, 1);

34270


34271
  /* '<S176>/Generated S-Function': DWORK2 */

34272
  ssSetDWorkName(S, 386, "DWORK386");

34273
  ssSetDWorkWidth(S, 386, 1);

34274
  ssSetDWorkDataType(S, 386, SS_INT8);

34275
  ssSetDWorkUsedAsDState(S, 386, 1);

34276


34277
  /* '<S176>/Generated S-Function': DWORK3 */

34278
  ssSetDWorkName(S, 387, "DWORK387");

34279
  ssSetDWorkWidth(S, 387, 1);

34280
  ssSetDWorkDataType(S, 387, SS_INT8);

34281
  ssSetDWorkUsedAsDState(S, 387, 1);

34282


34283
  /* '<S115>/Unit Delay': DSTATE */

34284
  ssSetDWorkName(S, 388, "DWORK388");

34285
  ssSetDWorkWidth(S, 388, 1);

34286
  ssSetDWorkDataType(S, 388, SS_INT8);

34287
  ssSetDWorkUsedAsDState(S, 388, 1);

34288


34289
  /* '<S89>/Generated S-Function3': DWORK2 */

34290
  ssSetDWorkName(S, 389, "DWORK389");

34291
  ssSetDWorkWidth(S, 389, 1);

34292
  ssSetDWorkDataType(S, 389, SS_INT8);

34293
  ssSetDWorkUsedAsDState(S, 389, 1);

34294


34295
  /* '<S89>/Generated S-Function3': DWORK3 */

34296
  ssSetDWorkName(S, 390, "DWORK390");

34297
  ssSetDWorkWidth(S, 390, 1);

34298
  ssSetDWorkDataType(S, 390, SS_INT8);

34299
  ssSetDWorkUsedAsDState(S, 390, 1);

34300


34301
  /* '<S90>/Generated S-Function2': DWORK2 */

34302
  ssSetDWorkName(S, 391, "DWORK391");

34303
  ssSetDWorkWidth(S, 391, 1);

34304
  ssSetDWorkDataType(S, 391, SS_INT8);

34305
  ssSetDWorkUsedAsDState(S, 391, 1);

34306


34307
  /* '<S90>/Generated S-Function2': DWORK3 */

34308
  ssSetDWorkName(S, 392, "DWORK392");

34309
  ssSetDWorkWidth(S, 392, 1);

34310
  ssSetDWorkDataType(S, 392, SS_INT8);

34311
  ssSetDWorkUsedAsDState(S, 392, 1);

34312


34313
  /* '<S87>/Generated S-Function4': DWORK2 */

34314
  ssSetDWorkName(S, 393, "DWORK393");

34315
  ssSetDWorkWidth(S, 393, 1);

34316
  ssSetDWorkDataType(S, 393, SS_INT8);

34317
  ssSetDWorkUsedAsDState(S, 393, 1);

34318


34319
  /* '<S87>/Generated S-Function4': DWORK3 */

34320
  ssSetDWorkName(S, 394, "DWORK394");

34321
  ssSetDWorkWidth(S, 394, 1);

34322
  ssSetDWorkDataType(S, 394, SS_INT8);

34323
  ssSetDWorkUsedAsDState(S, 394, 1);

34324


34325
  /* '<S113>/Generated S-Function': DWORK2 */

34326
  ssSetDWorkName(S, 395, "DWORK395");

34327
  ssSetDWorkWidth(S, 395, 1);

34328
  ssSetDWorkDataType(S, 395, SS_INT8);

34329
  ssSetDWorkUsedAsDState(S, 395, 1);

34330


34331
  /* '<S113>/Generated S-Function': DWORK3 */

34332
  ssSetDWorkName(S, 396, "DWORK396");

34333
  ssSetDWorkWidth(S, 396, 1);

34334
  ssSetDWorkDataType(S, 396, SS_INT8);

34335
  ssSetDWorkUsedAsDState(S, 396, 1);

34336


34337
  /* '<S118>/Generated S-Function2': DWORK2 */

34338
  ssSetDWorkName(S, 397, "DWORK397");

34339
  ssSetDWorkWidth(S, 397, 1);

34340
  ssSetDWorkDataType(S, 397, SS_INT8);

34341
  ssSetDWorkUsedAsDState(S, 397, 1);

34342


34343
  /* '<S118>/Generated S-Function2': DWORK3 */

34344
  ssSetDWorkName(S, 398, "DWORK398");

34345
  ssSetDWorkWidth(S, 398, 1);

34346
  ssSetDWorkDataType(S, 398, SS_INT8);

34347
  ssSetDWorkUsedAsDState(S, 398, 1);

34348


34349
  /* '<S117>/Generated S-Function4': DWORK2 */

34350
  ssSetDWorkName(S, 399, "DWORK399");

34351
  ssSetDWorkWidth(S, 399, 1);

34352
  ssSetDWorkDataType(S, 399, SS_INT8);

34353
  ssSetDWorkUsedAsDState(S, 399, 1);

34354


34355
  /* '<S117>/Generated S-Function4': DWORK3 */

34356
  ssSetDWorkName(S, 400, "DWORK400");

34357
  ssSetDWorkWidth(S, 400, 1);

34358
  ssSetDWorkDataType(S, 400, SS_INT8);

34359
  ssSetDWorkUsedAsDState(S, 400, 1);

34360


34361
  /* '<S116>/Generated S-Function4': DWORK2 */

34362
  ssSetDWorkName(S, 401, "DWORK401");

34363
  ssSetDWorkWidth(S, 401, 1);

34364
  ssSetDWorkDataType(S, 401, SS_INT8);

34365
  ssSetDWorkUsedAsDState(S, 401, 1);

34366


34367
  /* '<S116>/Generated S-Function4': DWORK3 */

34368
  ssSetDWorkName(S, 402, "DWORK402");

34369
  ssSetDWorkWidth(S, 402, 1);

34370
  ssSetDWorkDataType(S, 402, SS_INT8);

34371
  ssSetDWorkUsedAsDState(S, 402, 1);

34372


34373
  /* '<S100>/Generated S-Function3': DWORK2 */

34374
  ssSetDWorkName(S, 403, "DWORK403");

34375
  ssSetDWorkWidth(S, 403, 1);

34376
  ssSetDWorkDataType(S, 403, SS_INT8);

34377
  ssSetDWorkUsedAsDState(S, 403, 1);

34378


34379
  /* '<S100>/Generated S-Function3': DWORK3 */

34380
  ssSetDWorkName(S, 404, "DWORK404");

34381
  ssSetDWorkWidth(S, 404, 1);

34382
  ssSetDWorkDataType(S, 404, SS_INT8);

34383
  ssSetDWorkUsedAsDState(S, 404, 1);

34384


34385
  /* '<S101>/Generated S-Function2': DWORK2 */

34386
  ssSetDWorkName(S, 405, "DWORK405");

34387
  ssSetDWorkWidth(S, 405, 1);

34388
  ssSetDWorkDataType(S, 405, SS_INT8);

34389
  ssSetDWorkUsedAsDState(S, 405, 1);

34390


34391
  /* '<S101>/Generated S-Function2': DWORK3 */

34392
  ssSetDWorkName(S, 406, "DWORK406");

34393
  ssSetDWorkWidth(S, 406, 1);

34394
  ssSetDWorkDataType(S, 406, SS_INT8);

34395
  ssSetDWorkUsedAsDState(S, 406, 1);

34396


34397
  /* '<S98>/Generated S-Function4': DWORK2 */

34398
  ssSetDWorkName(S, 407, "DWORK407");

34399
  ssSetDWorkWidth(S, 407, 1);

34400
  ssSetDWorkDataType(S, 407, SS_INT8);

34401
  ssSetDWorkUsedAsDState(S, 407, 1);

34402


34403
  /* '<S98>/Generated S-Function4': DWORK3 */

34404
  ssSetDWorkName(S, 408, "DWORK408");

34405
  ssSetDWorkWidth(S, 408, 1);

34406
  ssSetDWorkDataType(S, 408, SS_INT8);

34407
  ssSetDWorkUsedAsDState(S, 408, 1);

34408


34409
  /* '<S107>/Generated S-Function3': DWORK2 */

34410
  ssSetDWorkName(S, 409, "DWORK409");

34411
  ssSetDWorkWidth(S, 409, 1);

34412
  ssSetDWorkDataType(S, 409, SS_INT8);

34413
  ssSetDWorkUsedAsDState(S, 409, 1);

34414


34415
  /* '<S107>/Generated S-Function3': DWORK3 */

34416
  ssSetDWorkName(S, 410, "DWORK410");

34417
  ssSetDWorkWidth(S, 410, 1);

34418
  ssSetDWorkDataType(S, 410, SS_INT8);

34419
  ssSetDWorkUsedAsDState(S, 410, 1);

34420


34421
  /* '<S108>/Generated S-Function2': DWORK2 */

34422
  ssSetDWorkName(S, 411, "DWORK411");

34423
  ssSetDWorkWidth(S, 411, 1);

34424
  ssSetDWorkDataType(S, 411, SS_INT8);

34425
  ssSetDWorkUsedAsDState(S, 411, 1);

34426


34427
  /* '<S108>/Generated S-Function2': DWORK3 */

34428
  ssSetDWorkName(S, 412, "DWORK412");

34429
  ssSetDWorkWidth(S, 412, 1);

34430
  ssSetDWorkDataType(S, 412, SS_INT8);

34431
  ssSetDWorkUsedAsDState(S, 412, 1);

34432


34433
  /* '<S106>/Generated S-Function4': DWORK2 */

34434
  ssSetDWorkName(S, 413, "DWORK413");

34435
  ssSetDWorkWidth(S, 413, 1);

34436
  ssSetDWorkDataType(S, 413, SS_INT8);

34437
  ssSetDWorkUsedAsDState(S, 413, 1);

34438


34439
  /* '<S106>/Generated S-Function4': DWORK3 */

34440
  ssSetDWorkName(S, 414, "DWORK414");

34441
  ssSetDWorkWidth(S, 414, 1);

34442
  ssSetDWorkDataType(S, 414, SS_INT8);

34443
  ssSetDWorkUsedAsDState(S, 414, 1);

34444


34445
  /* '<S56>/Unit Delay': DSTATE */

34446
  ssSetDWorkName(S, 415, "DWORK415");

34447
  ssSetDWorkWidth(S, 415, 1);

34448
  ssSetDWorkDataType(S, 415, SS_INT8);

34449
  ssSetDWorkUsedAsDState(S, 415, 1);

34450


34451
  /* '<S138>/Unit Delay': DSTATE */

34452
  ssSetDWorkName(S, 416, "DWORK416");

34453
  ssSetDWorkWidth(S, 416, 1);

34454
  ssSetDWorkDataType(S, 416, SS_INT8);

34455
  ssSetDWorkUsedAsDState(S, 416, 1);

34456


34457
  /* '<S130>/Generated S-Function4': DWORK2 */

34458
  ssSetDWorkName(S, 417, "DWORK417");

34459
  ssSetDWorkWidth(S, 417, 1);

34460
  ssSetDWorkDataType(S, 417, SS_INT8);

34461
  ssSetDWorkUsedAsDState(S, 417, 1);

34462


34463
  /* '<S130>/Generated S-Function4': DWORK3 */

34464
  ssSetDWorkName(S, 418, "DWORK418");

34465
  ssSetDWorkWidth(S, 418, 1);

34466
  ssSetDWorkDataType(S, 418, SS_INT8);

34467
  ssSetDWorkUsedAsDState(S, 418, 1);

34468


34469
  /* '<S132>/Generated S-Function3': DWORK2 */

34470
  ssSetDWorkName(S, 419, "DWORK419");

34471
  ssSetDWorkWidth(S, 419, 1);

34472
  ssSetDWorkDataType(S, 419, SS_INT8);

34473
  ssSetDWorkUsedAsDState(S, 419, 1);

34474


34475
  /* '<S132>/Generated S-Function3': DWORK3 */

34476
  ssSetDWorkName(S, 420, "DWORK420");

34477
  ssSetDWorkWidth(S, 420, 1);

34478
  ssSetDWorkDataType(S, 420, SS_INT8);

34479
  ssSetDWorkUsedAsDState(S, 420, 1);

34480


34481
  /* '<S133>/Generated S-Function2': DWORK2 */

34482
  ssSetDWorkName(S, 421, "DWORK421");

34483
  ssSetDWorkWidth(S, 421, 1);

34484
  ssSetDWorkDataType(S, 421, SS_INT8);

34485
  ssSetDWorkUsedAsDState(S, 421, 1);

34486


34487
  /* '<S133>/Generated S-Function2': DWORK3 */

34488
  ssSetDWorkName(S, 422, "DWORK422");

34489
  ssSetDWorkWidth(S, 422, 1);

34490
  ssSetDWorkDataType(S, 422, SS_INT8);

34491
  ssSetDWorkUsedAsDState(S, 422, 1);

34492


34493
  /* '<S131>/Generated S-Function4': DWORK2 */

34494
  ssSetDWorkName(S, 423, "DWORK423");

34495
  ssSetDWorkWidth(S, 423, 1);

34496
  ssSetDWorkDataType(S, 423, SS_INT8);

34497
  ssSetDWorkUsedAsDState(S, 423, 1);

34498


34499
  /* '<S131>/Generated S-Function4': DWORK3 */

34500
  ssSetDWorkName(S, 424, "DWORK424");

34501
  ssSetDWorkWidth(S, 424, 1);

34502
  ssSetDWorkDataType(S, 424, SS_INT8);

34503
  ssSetDWorkUsedAsDState(S, 424, 1);

34504


34505
  /* '<S152>/Generated S-Function2': DWORK2 */

34506
  ssSetDWorkName(S, 425, "DWORK425");

34507
  ssSetDWorkWidth(S, 425, 1);

34508
  ssSetDWorkDataType(S, 425, SS_INT8);

34509
  ssSetDWorkUsedAsDState(S, 425, 1);

34510


34511
  /* '<S152>/Generated S-Function2': DWORK3 */

34512
  ssSetDWorkName(S, 426, "DWORK426");

34513
  ssSetDWorkWidth(S, 426, 1);

34514
  ssSetDWorkDataType(S, 426, SS_INT8);

34515
  ssSetDWorkUsedAsDState(S, 426, 1);

34516


34517
  /* '<S150>/Generated S-Function3': DWORK2 */

34518
  ssSetDWorkName(S, 427, "DWORK427");

34519
  ssSetDWorkWidth(S, 427, 1);

34520
  ssSetDWorkDataType(S, 427, SS_INT8);

34521
  ssSetDWorkUsedAsDState(S, 427, 1);

34522


34523
  /* '<S150>/Generated S-Function3': DWORK3 */

34524
  ssSetDWorkName(S, 428, "DWORK428");

34525
  ssSetDWorkWidth(S, 428, 1);

34526
  ssSetDWorkDataType(S, 428, SS_INT8);

34527
  ssSetDWorkUsedAsDState(S, 428, 1);

34528


34529
  /* '<S151>/Generated S-Function2': DWORK2 */

34530
  ssSetDWorkName(S, 429, "DWORK429");

34531
  ssSetDWorkWidth(S, 429, 1);

34532
  ssSetDWorkDataType(S, 429, SS_INT8);

34533
  ssSetDWorkUsedAsDState(S, 429, 1);

34534


34535
  /* '<S151>/Generated S-Function2': DWORK3 */

34536
  ssSetDWorkName(S, 430, "DWORK430");

34537
  ssSetDWorkWidth(S, 430, 1);

34538
  ssSetDWorkDataType(S, 430, SS_INT8);

34539
  ssSetDWorkUsedAsDState(S, 430, 1);

34540


34541
  /* '<S149>/Generated S-Function4': DWORK2 */

34542
  ssSetDWorkName(S, 431, "DWORK431");

34543
  ssSetDWorkWidth(S, 431, 1);

34544
  ssSetDWorkDataType(S, 431, SS_INT8);

34545
  ssSetDWorkUsedAsDState(S, 431, 1);

34546


34547
  /* '<S149>/Generated S-Function4': DWORK3 */

34548
  ssSetDWorkName(S, 432, "DWORK432");

34549
  ssSetDWorkWidth(S, 432, 1);

34550
  ssSetDWorkDataType(S, 432, SS_INT8);

34551
  ssSetDWorkUsedAsDState(S, 432, 1);

34552


34553
  /* '<S142>/Generated S-Function2': DWORK2 */

34554
  ssSetDWorkName(S, 433, "DWORK433");

34555
  ssSetDWorkWidth(S, 433, 1);

34556
  ssSetDWorkDataType(S, 433, SS_INT8);

34557
  ssSetDWorkUsedAsDState(S, 433, 1);

34558


34559
  /* '<S142>/Generated S-Function2': DWORK3 */

34560
  ssSetDWorkName(S, 434, "DWORK434");

34561
  ssSetDWorkWidth(S, 434, 1);

34562
  ssSetDWorkDataType(S, 434, SS_INT8);

34563
  ssSetDWorkUsedAsDState(S, 434, 1);

34564


34565
  /* '<S140>/Generated S-Function3': DWORK2 */

34566
  ssSetDWorkName(S, 435, "DWORK435");

34567
  ssSetDWorkWidth(S, 435, 1);

34568
  ssSetDWorkDataType(S, 435, SS_INT8);

34569
  ssSetDWorkUsedAsDState(S, 435, 1);

34570


34571
  /* '<S140>/Generated S-Function3': DWORK3 */

34572
  ssSetDWorkName(S, 436, "DWORK436");

34573
  ssSetDWorkWidth(S, 436, 1);

34574
  ssSetDWorkDataType(S, 436, SS_INT8);

34575
  ssSetDWorkUsedAsDState(S, 436, 1);

34576


34577
  /* '<S141>/Generated S-Function2': DWORK2 */

34578
  ssSetDWorkName(S, 437, "DWORK437");

34579
  ssSetDWorkWidth(S, 437, 1);

34580
  ssSetDWorkDataType(S, 437, SS_INT8);

34581
  ssSetDWorkUsedAsDState(S, 437, 1);

34582


34583
  /* '<S141>/Generated S-Function2': DWORK3 */

34584
  ssSetDWorkName(S, 438, "DWORK438");

34585
  ssSetDWorkWidth(S, 438, 1);

34586
  ssSetDWorkDataType(S, 438, SS_INT8);

34587
  ssSetDWorkUsedAsDState(S, 438, 1);

34588


34589
  /* '<S139>/Generated S-Function4': DWORK2 */

34590
  ssSetDWorkName(S, 439, "DWORK439");

34591
  ssSetDWorkWidth(S, 439, 1);

34592
  ssSetDWorkDataType(S, 439, SS_INT8);

34593
  ssSetDWorkUsedAsDState(S, 439, 1);

34594


34595
  /* '<S139>/Generated S-Function4': DWORK3 */

34596
  ssSetDWorkName(S, 440, "DWORK440");

34597
  ssSetDWorkWidth(S, 440, 1);

34598
  ssSetDWorkDataType(S, 440, SS_INT8);

34599
  ssSetDWorkUsedAsDState(S, 440, 1);

34600


34601
  /* '<S160>/Generated S-Function3': DWORK2 */

34602
  ssSetDWorkName(S, 441, "DWORK441");

34603
  ssSetDWorkWidth(S, 441, 1);

34604
  ssSetDWorkDataType(S, 441, SS_INT8);

34605
  ssSetDWorkUsedAsDState(S, 441, 1);

34606


34607
  /* '<S160>/Generated S-Function3': DWORK3 */

34608
  ssSetDWorkName(S, 442, "DWORK442");

34609
  ssSetDWorkWidth(S, 442, 1);

34610
  ssSetDWorkDataType(S, 442, SS_INT8);

34611
  ssSetDWorkUsedAsDState(S, 442, 1);

34612


34613
  /* '<S159>/Generated S-Function4': DWORK2 */

34614
  ssSetDWorkName(S, 443, "DWORK443");

34615
  ssSetDWorkWidth(S, 443, 1);

34616
  ssSetDWorkDataType(S, 443, SS_INT8);

34617
  ssSetDWorkUsedAsDState(S, 443, 1);

34618


34619
  /* '<S159>/Generated S-Function4': DWORK3 */

34620
  ssSetDWorkName(S, 444, "DWORK444");

34621
  ssSetDWorkWidth(S, 444, 1);

34622
  ssSetDWorkDataType(S, 444, SS_INT8);

34623
  ssSetDWorkUsedAsDState(S, 444, 1);

34624


34625
  /* '<S195>/Unit Delay': DSTATE */

34626
  ssSetDWorkName(S, 445, "DWORK445");

34627
  ssSetDWorkWidth(S, 445, 1);

34628
  ssSetDWorkDataType(S, 445, SS_INT8);

34629
  ssSetDWorkUsedAsDState(S, 445, 1);

34630


34631
  /* '<S190>/Generated S-Function4': DWORK2 */

34632
  ssSetDWorkName(S, 446, "DWORK446");

34633
  ssSetDWorkWidth(S, 446, 1);

34634
  ssSetDWorkDataType(S, 446, SS_INT8);

34635
  ssSetDWorkUsedAsDState(S, 446, 1);

34636


34637
  /* '<S190>/Generated S-Function4': DWORK3 */

34638
  ssSetDWorkName(S, 447, "DWORK447");

34639
  ssSetDWorkWidth(S, 447, 1);

34640
  ssSetDWorkDataType(S, 447, SS_INT8);

34641
  ssSetDWorkUsedAsDState(S, 447, 1);

34642


34643
  /* '<S191>/Generated S-Function4': DWORK2 */

34644
  ssSetDWorkName(S, 448, "DWORK448");

34645
  ssSetDWorkWidth(S, 448, 1);

34646
  ssSetDWorkDataType(S, 448, SS_INT8);

34647
  ssSetDWorkUsedAsDState(S, 448, 1);

34648


34649
  /* '<S191>/Generated S-Function4': DWORK3 */

34650
  ssSetDWorkName(S, 449, "DWORK449");

34651
  ssSetDWorkWidth(S, 449, 1);

34652
  ssSetDWorkDataType(S, 449, SS_INT8);

34653
  ssSetDWorkUsedAsDState(S, 449, 1);

34654


34655
  /* '<S197>/Generated S-Function2': DWORK2 */

34656
  ssSetDWorkName(S, 450, "DWORK450");

34657
  ssSetDWorkWidth(S, 450, 1);

34658
  ssSetDWorkDataType(S, 450, SS_INT8);

34659
  ssSetDWorkUsedAsDState(S, 450, 1);

34660


34661
  /* '<S197>/Generated S-Function2': DWORK3 */

34662
  ssSetDWorkName(S, 451, "DWORK451");

34663
  ssSetDWorkWidth(S, 451, 1);

34664
  ssSetDWorkDataType(S, 451, SS_INT8);

34665
  ssSetDWorkUsedAsDState(S, 451, 1);

34666


34667
  /* '<S196>/Generated S-Function4': DWORK2 */

34668
  ssSetDWorkName(S, 452, "DWORK452");

34669
  ssSetDWorkWidth(S, 452, 1);

34670
  ssSetDWorkDataType(S, 452, SS_INT8);

34671
  ssSetDWorkUsedAsDState(S, 452, 1);

34672


34673
  /* '<S196>/Generated S-Function4': DWORK3 */

34674
  ssSetDWorkName(S, 453, "DWORK453");

34675
  ssSetDWorkWidth(S, 453, 1);

34676
  ssSetDWorkDataType(S, 453, SS_INT8);

34677
  ssSetDWorkUsedAsDState(S, 453, 1);

34678


34679
  /* '<S201>/Generated S-Function2': DWORK2 */

34680
  ssSetDWorkName(S, 454, "DWORK454");

34681
  ssSetDWorkWidth(S, 454, 1);

34682
  ssSetDWorkDataType(S, 454, SS_INT8);

34683
  ssSetDWorkUsedAsDState(S, 454, 1);

34684


34685
  /* '<S201>/Generated S-Function2': DWORK3 */

34686
  ssSetDWorkName(S, 455, "DWORK455");

34687
  ssSetDWorkWidth(S, 455, 1);

34688
  ssSetDWorkDataType(S, 455, SS_INT8);

34689
  ssSetDWorkUsedAsDState(S, 455, 1);

34690


34691
  /* '<S200>/Generated S-Function4': DWORK2 */

34692
  ssSetDWorkName(S, 456, "DWORK456");

34693
  ssSetDWorkWidth(S, 456, 1);

34694
  ssSetDWorkDataType(S, 456, SS_INT8);

34695
  ssSetDWorkUsedAsDState(S, 456, 1);

34696


34697
  /* '<S200>/Generated S-Function4': DWORK3 */

34698
  ssSetDWorkName(S, 457, "DWORK457");

34699
  ssSetDWorkWidth(S, 457, 1);

34700
  ssSetDWorkDataType(S, 457, SS_INT8);

34701
  ssSetDWorkUsedAsDState(S, 457, 1);

34702


34703
  /* '<S73>/Unit Delay': DSTATE */

34704
  ssSetDWorkName(S, 458, "DWORK458");

34705
  ssSetDWorkWidth(S, 458, 1);

34706
  ssSetDWorkDataType(S, 458, SS_INT8);

34707
  ssSetDWorkUsedAsDState(S, 458, 1);

34708


34709
  /* '<S69>/Generated S-Function4': DWORK2 */

34710
  ssSetDWorkName(S, 459, "DWORK459");

34711
  ssSetDWorkWidth(S, 459, 1);

34712
  ssSetDWorkDataType(S, 459, SS_INT8);

34713
  ssSetDWorkUsedAsDState(S, 459, 1);

34714


34715
  /* '<S69>/Generated S-Function4': DWORK3 */

34716
  ssSetDWorkName(S, 460, "DWORK460");

34717
  ssSetDWorkWidth(S, 460, 1);

34718
  ssSetDWorkDataType(S, 460, SS_INT8);

34719
  ssSetDWorkUsedAsDState(S, 460, 1);

34720


34721
  /* '<S70>/Generated S-Function4': DWORK2 */

34722
  ssSetDWorkName(S, 461, "DWORK461");

34723
  ssSetDWorkWidth(S, 461, 1);

34724
  ssSetDWorkDataType(S, 461, SS_INT8);

34725
  ssSetDWorkUsedAsDState(S, 461, 1);

34726


34727
  /* '<S70>/Generated S-Function4': DWORK3 */

34728
  ssSetDWorkName(S, 462, "DWORK462");

34729
  ssSetDWorkWidth(S, 462, 1);

34730
  ssSetDWorkDataType(S, 462, SS_INT8);

34731
  ssSetDWorkUsedAsDState(S, 462, 1);

34732


34733
  /* '<S75>/Generated S-Function2': DWORK2 */

34734
  ssSetDWorkName(S, 463, "DWORK463");

34735
  ssSetDWorkWidth(S, 463, 1);

34736
  ssSetDWorkDataType(S, 463, SS_INT8);

34737
  ssSetDWorkUsedAsDState(S, 463, 1);

34738


34739
  /* '<S75>/Generated S-Function2': DWORK3 */

34740
  ssSetDWorkName(S, 464, "DWORK464");

34741
  ssSetDWorkWidth(S, 464, 1);

34742
  ssSetDWorkDataType(S, 464, SS_INT8);

34743
  ssSetDWorkUsedAsDState(S, 464, 1);

34744


34745
  /* '<S74>/Generated S-Function4': DWORK2 */

34746
  ssSetDWorkName(S, 465, "DWORK465");

34747
  ssSetDWorkWidth(S, 465, 1);

34748
  ssSetDWorkDataType(S, 465, SS_INT8);

34749
  ssSetDWorkUsedAsDState(S, 465, 1);

34750


34751
  /* '<S74>/Generated S-Function4': DWORK3 */

34752
  ssSetDWorkName(S, 466, "DWORK466");

34753
  ssSetDWorkWidth(S, 466, 1);

34754
  ssSetDWorkDataType(S, 466, SS_INT8);

34755
  ssSetDWorkUsedAsDState(S, 466, 1);

34756


34757
  /* '<S208>/Generated S-Function': DWORK1 */

34758
  ssSetDWorkName(S, 467, "DWORK467");

34759
  ssSetDWorkWidth(S, 467, 1);

34760
  ssSetDWorkDataType(S, 467, SS_INT8);

34761
  ssSetDWorkUsedAsDState(S, 467, 1);

34762


34763
  /* '<S208>/Generated S-Function': DWORK2 */

34764
  ssSetDWorkName(S, 468, "DWORK468");

34765
  ssSetDWorkWidth(S, 468, 1);

34766
  ssSetDWorkDataType(S, 468, SS_INT8);

34767
  ssSetDWorkUsedAsDState(S, 468, 1);

34768


34769
  /* '<S241>/Generated S-Function4': DWORK2 */

34770
  ssSetDWorkName(S, 469, "DWORK469");

34771
  ssSetDWorkWidth(S, 469, 1);

34772
  ssSetDWorkDataType(S, 469, SS_INT8);

34773
  ssSetDWorkUsedAsDState(S, 469, 1);

34774


34775
  /* '<S241>/Generated S-Function4': DWORK3 */

34776
  ssSetDWorkName(S, 470, "DWORK470");

34777
  ssSetDWorkWidth(S, 470, 1);

34778
  ssSetDWorkDataType(S, 470, SS_INT8);

34779
  ssSetDWorkUsedAsDState(S, 470, 1);

34780


34781
  /* '<S239>/Generated S-Function4': DWORK2 */

34782
  ssSetDWorkName(S, 471, "DWORK471");

34783
  ssSetDWorkWidth(S, 471, 1);

34784
  ssSetDWorkDataType(S, 471, SS_INT8);

34785
  ssSetDWorkUsedAsDState(S, 471, 1);

34786


34787
  /* '<S239>/Generated S-Function4': DWORK3 */

34788
  ssSetDWorkName(S, 472, "DWORK472");

34789
  ssSetDWorkWidth(S, 472, 1);

34790
  ssSetDWorkDataType(S, 472, SS_INT8);

34791
  ssSetDWorkUsedAsDState(S, 472, 1);

34792


34793
  /* '<S240>/Generated S-Function4': DWORK2 */

34794
  ssSetDWorkName(S, 473, "DWORK473");

34795
  ssSetDWorkWidth(S, 473, 1);

34796
  ssSetDWorkDataType(S, 473, SS_INT8);

34797
  ssSetDWorkUsedAsDState(S, 473, 1);

34798


34799
  /* '<S240>/Generated S-Function4': DWORK3 */

34800
  ssSetDWorkName(S, 474, "DWORK474");

34801
  ssSetDWorkWidth(S, 474, 1);

34802
  ssSetDWorkDataType(S, 474, SS_INT8);

34803
  ssSetDWorkUsedAsDState(S, 474, 1);

34804


34805
  /* '<S238>/Generated S-Function8': DWORK2 */

34806
  ssSetDWorkName(S, 475, "DWORK475");

34807
  ssSetDWorkWidth(S, 475, 1);

34808
  ssSetDWorkDataType(S, 475, SS_INT8);

34809
  ssSetDWorkUsedAsDState(S, 475, 1);

34810


34811
  /* '<S238>/Generated S-Function8': DWORK3 */

34812
  ssSetDWorkName(S, 476, "DWORK476");

34813
  ssSetDWorkWidth(S, 476, 1);

34814
  ssSetDWorkDataType(S, 476, SS_INT8);

34815
  ssSetDWorkUsedAsDState(S, 476, 1);

34816


34817
  /* '<S223>/Generated S-Function7': DWORK1 */

34818
  ssSetDWorkName(S, 477, "DWORK477");

34819
  ssSetDWorkWidth(S, 477, 1);

34820
  ssSetDWorkDataType(S, 477, SS_INT8);

34821
  ssSetDWorkUsedAsDState(S, 477, 1);

34822


34823
  /* '<S223>/Generated S-Function7': DWORK2 */

34824
  ssSetDWorkName(S, 478, "DWORK478");

34825
  ssSetDWorkWidth(S, 478, 1);

34826
  ssSetDWorkDataType(S, 478, SS_INT8);

34827
  ssSetDWorkUsedAsDState(S, 478, 1);

34828


34829
  /* '<S224>/Generated S-Function7': DWORK1 */

34830
  ssSetDWorkName(S, 479, "DWORK479");

34831
  ssSetDWorkWidth(S, 479, 1);

34832
  ssSetDWorkDataType(S, 479, SS_INT8);

34833
  ssSetDWorkUsedAsDState(S, 479, 1);

34834


34835
  /* '<S224>/Generated S-Function7': DWORK2 */

34836
  ssSetDWorkName(S, 480, "DWORK480");

34837
  ssSetDWorkWidth(S, 480, 1);

34838
  ssSetDWorkDataType(S, 480, SS_INT8);

34839
  ssSetDWorkUsedAsDState(S, 480, 1);

34840


34841
  /* '<S252>/Generated S-Function8': DWORK2 */

34842
  ssSetDWorkName(S, 481, "DWORK481");

34843
  ssSetDWorkWidth(S, 481, 1);

34844
  ssSetDWorkDataType(S, 481, SS_INT8);

34845
  ssSetDWorkUsedAsDState(S, 481, 1);

34846


34847
  /* '<S252>/Generated S-Function8': DWORK3 */

34848
  ssSetDWorkName(S, 482, "DWORK482");

34849
  ssSetDWorkWidth(S, 482, 1);

34850
  ssSetDWorkDataType(S, 482, SS_INT8);

34851
  ssSetDWorkUsedAsDState(S, 482, 1);

34852


34853
  /* '<S253>/Generated S-Function4': DWORK2 */

34854
  ssSetDWorkName(S, 483, "DWORK483");

34855
  ssSetDWorkWidth(S, 483, 1);

34856
  ssSetDWorkDataType(S, 483, SS_INT8);

34857
  ssSetDWorkUsedAsDState(S, 483, 1);

34858


34859
  /* '<S253>/Generated S-Function4': DWORK3 */

34860
  ssSetDWorkName(S, 484, "DWORK484");

34861
  ssSetDWorkWidth(S, 484, 1);

34862
  ssSetDWorkDataType(S, 484, SS_INT8);

34863
  ssSetDWorkUsedAsDState(S, 484, 1);

34864


34865
  /* '<S254>/Generated S-Function4': DWORK2 */

34866
  ssSetDWorkName(S, 485, "DWORK485");

34867
  ssSetDWorkWidth(S, 485, 1);

34868
  ssSetDWorkDataType(S, 485, SS_INT8);

34869
  ssSetDWorkUsedAsDState(S, 485, 1);

34870


34871
  /* '<S254>/Generated S-Function4': DWORK3 */

34872
  ssSetDWorkName(S, 486, "DWORK486");

34873
  ssSetDWorkWidth(S, 486, 1);

34874
  ssSetDWorkDataType(S, 486, SS_INT8);

34875
  ssSetDWorkUsedAsDState(S, 486, 1);

34876


34877
  /* '<S266>/Generated S-Function': DWORK2 */

34878
  ssSetDWorkName(S, 487, "DWORK487");

34879
  ssSetDWorkWidth(S, 487, 1);

34880
  ssSetDWorkDataType(S, 487, SS_INT8);

34881
  ssSetDWorkUsedAsDState(S, 487, 1);

34882


34883
  /* '<S266>/Generated S-Function': DWORK3 */

34884
  ssSetDWorkName(S, 488, "DWORK488");

34885
  ssSetDWorkWidth(S, 488, 1);

34886
  ssSetDWorkDataType(S, 488, SS_INT8);

34887
  ssSetDWorkUsedAsDState(S, 488, 1);

34888


34889
  /* '<S265>/Generated S-Function8': DWORK2 */

34890
  ssSetDWorkName(S, 489, "DWORK489");

34891
  ssSetDWorkWidth(S, 489, 1);

34892
  ssSetDWorkDataType(S, 489, SS_INT8);

34893
  ssSetDWorkUsedAsDState(S, 489, 1);

34894


34895
  /* '<S265>/Generated S-Function8': DWORK3 */

34896
  ssSetDWorkName(S, 490, "DWORK490");

34897
  ssSetDWorkWidth(S, 490, 1);

34898
  ssSetDWorkDataType(S, 490, SS_INT8);

34899
  ssSetDWorkUsedAsDState(S, 490, 1);

34900


34901
  /* '<S273>/Unit Delay': DSTATE */

34902
  ssSetDWorkName(S, 491, "DWORK491");

34903
  ssSetDWorkWidth(S, 491, 1);

34904
  ssSetDWorkDataType(S, 491, SS_INT8);

34905
  ssSetDWorkUsedAsDState(S, 491, 1);

34906


34907
  /* '<S257>/Generated S-Function3': DWORK2 */

34908
  ssSetDWorkName(S, 492, "DWORK492");

34909
  ssSetDWorkWidth(S, 492, 1);

34910
  ssSetDWorkDataType(S, 492, SS_INT8);

34911
  ssSetDWorkUsedAsDState(S, 492, 1);

34912


34913
  /* '<S257>/Generated S-Function3': DWORK3 */

34914
  ssSetDWorkName(S, 493, "DWORK493");

34915
  ssSetDWorkWidth(S, 493, 1);

34916
  ssSetDWorkDataType(S, 493, SS_INT8);

34917
  ssSetDWorkUsedAsDState(S, 493, 1);

34918


34919
  /* '<S249>/Generated S-Function7': DWORK1 */

34920
  ssSetDWorkName(S, 494, "DWORK494");

34921
  ssSetDWorkWidth(S, 494, 1);

34922
  ssSetDWorkDataType(S, 494, SS_INT8);

34923
  ssSetDWorkUsedAsDState(S, 494, 1);

34924


34925
  /* '<S249>/Generated S-Function7': DWORK2 */

34926
  ssSetDWorkName(S, 495, "DWORK495");

34927
  ssSetDWorkWidth(S, 495, 1);

34928
  ssSetDWorkDataType(S, 495, SS_INT8);

34929
  ssSetDWorkUsedAsDState(S, 495, 1);

34930


34931
  /* '<S274>/Generated S-Function7': DWORK1 */

34932
  ssSetDWorkName(S, 496, "DWORK496");

34933
  ssSetDWorkWidth(S, 496, 1);

34934
  ssSetDWorkDataType(S, 496, SS_INT8);

34935
  ssSetDWorkUsedAsDState(S, 496, 1);

34936


34937
  /* '<S274>/Generated S-Function7': DWORK2 */

34938
  ssSetDWorkName(S, 497, "DWORK497");

34939
  ssSetDWorkWidth(S, 497, 1);

34940
  ssSetDWorkDataType(S, 497, SS_INT8);

34941
  ssSetDWorkUsedAsDState(S, 497, 1);

34942


34943
  /* '<S215>/Generated S-Function4': DWORK2 */

34944
  ssSetDWorkName(S, 498, "DWORK498");

34945
  ssSetDWorkWidth(S, 498, 1);

34946
  ssSetDWorkDataType(S, 498, SS_INT8);

34947
  ssSetDWorkUsedAsDState(S, 498, 1);

34948


34949
  /* '<S215>/Generated S-Function4': DWORK3 */

34950
  ssSetDWorkName(S, 499, "DWORK499");

34951
  ssSetDWorkWidth(S, 499, 1);

34952
  ssSetDWorkDataType(S, 499, SS_INT8);

34953
  ssSetDWorkUsedAsDState(S, 499, 1);

34954


34955
  /* '<S216>/Generated S-Function7': DWORK1 */

34956
  ssSetDWorkName(S, 500, "DWORK500");

34957
  ssSetDWorkWidth(S, 500, 1);

34958
  ssSetDWorkDataType(S, 500, SS_INT8);

34959
  ssSetDWorkUsedAsDState(S, 500, 1);

34960


34961
  /* '<S216>/Generated S-Function7': DWORK2 */

34962
  ssSetDWorkName(S, 501, "DWORK501");

34963
  ssSetDWorkWidth(S, 501, 1);

34964
  ssSetDWorkDataType(S, 501, SS_INT8);

34965
  ssSetDWorkUsedAsDState(S, 501, 1);

34966


34967
  /* '<S310>/Delay': DSTATE */

34968
  ssSetDWorkName(S, 502, "DWORK502");

34969
  ssSetDWorkWidth(S, 502, 1);

34970
  ssSetDWorkDataType(S, 502, SS_BOOLEAN);

34971
  ssSetDWorkUsedAsDState(S, 502, 1);

34972


34973
  /* '<S431>/Delay': DSTATE */

34974
  ssSetDWorkName(S, 503, "DWORK503");

34975
  ssSetDWorkWidth(S, 503, 1);

34976
  ssSetDWorkDataType(S, 503, SS_BOOLEAN);

34977
  ssSetDWorkUsedAsDState(S, 503, 1);

34978


34979
  /* '<S9>/Memory2': PreviousInput */

34980
  ssSetDWorkName(S, 504, "DWORK504");

34981
  ssSetDWorkWidth(S, 504, 1);

34982
  ssSetDWorkDataType(S, 504, SS_INT8);

34983


34984
  /* '<S31>/Switch Case': ActiveSubsystem */

34985
  ssSetDWorkName(S, 505, "DWORK505");

34986
  ssSetDWorkWidth(S, 505, 1);

34987
  ssSetDWorkDataType(S, 505, SS_INT8);

34988


34989
  /* '<S847>/If': ActiveSubsystem */

34990
  ssSetDWorkName(S, 506, "DWORK506");

34991
  ssSetDWorkWidth(S, 506, 1);

34992
  ssSetDWorkDataType(S, 506, SS_INT8);

34993


34994
  /* '<S846>/If': ActiveSubsystem */

34995
  ssSetDWorkName(S, 507, "DWORK507");

34996
  ssSetDWorkWidth(S, 507, 1);

34997
  ssSetDWorkDataType(S, 507, SS_INT8);

34998


34999
  /* '<S848>/If': ActiveSubsystem */

35000
  ssSetDWorkName(S, 508, "DWORK508");

35001
  ssSetDWorkWidth(S, 508, 1);

35002
  ssSetDWorkDataType(S, 508, SS_INT8);

35003


35004
  /* '<S902>/Find Maximum Diagonal Value': ActiveSubsystem */

35005
  ssSetDWorkName(S, 509, "DWORK509");

35006
  ssSetDWorkWidth(S, 509, 1);

35007
  ssSetDWorkDataType(S, 509, SS_INT8);

35008


35009
  /* '<S877>/Find Maximum Diagonal Value': ActiveSubsystem */

35010
  ssSetDWorkName(S, 510, "DWORK510");

35011
  ssSetDWorkWidth(S, 510, 1);

35012
  ssSetDWorkDataType(S, 510, SS_INT8);

35013


35014
  /* '<S316>/Landing2': DWORK25 */

35015
  ssSetDWorkName(S, 511, "DWORK511");

35016
  ssSetDWorkWidth(S, 511, 1);

35017
  ssSetDWorkDataType(S, 511, SS_INT8);

35018


35019
  /* '<S316>/Landing2': DWORK30 */

35020
  ssSetDWorkName(S, 512, "DWORK512");

35021
  ssSetDWorkWidth(S, 512, 1);

35022
  ssSetDWorkDataType(S, 512, SS_INT8);

35023


35024
  /* '<S313>/Approach2': DWORK15 */

35025
  ssSetDWorkName(S, 513, "DWORK513");

35026
  ssSetDWorkWidth(S, 513, 1);

35027
  ssSetDWorkDataType(S, 513, SS_INT8);

35028


35029
  /* '<S31>/LogicTrans': DWORK1 */

35030
  ssSetDWorkName(S, 514, "DWORK514");

35031
  ssSetDWorkWidth(S, 514, 1);

35032
  ssSetDWorkDataType(S, 514, SS_INT8);

35033


35034
  /* '<S299>/AirLineUpdateSucModel': DWORK4 */

35035
  ssSetDWorkName(S, 515, "DWORK515");

35036
  ssSetDWorkWidth(S, 515, 1);

35037
  ssSetDWorkDataType(S, 515, SS_INT8);

35038


35039
  /* '<S293>/CompTP': DWORK3 */

35040
  ssSetDWorkName(S, 516, "DWORK516");

35041
  ssSetDWorkWidth(S, 516, 1);

35042
  ssSetDWorkDataType(S, 516, SS_INT8);

35043


35044
  /* '<S81>/Generated S-Function5': DWORK5 */

35045
  ssSetDWorkName(S, 517, "DWORK517");

35046
  ssSetDWorkWidth(S, 517, 1);

35047
  ssSetDWorkDataType(S, 517, SS_INT8);

35048


35049
  /* '<S81>/Generated S-Function5': DWORK6 */

35050
  ssSetDWorkName(S, 518, "DWORK518");

35051
  ssSetDWorkWidth(S, 518, 1);

35052
  ssSetDWorkDataType(S, 518, SS_INT8);

35053


35054
  /* '<S123>/Generated S-Function5': DWORK5 */

35055
  ssSetDWorkName(S, 519, "DWORK519");

35056
  ssSetDWorkWidth(S, 519, 1);

35057
  ssSetDWorkDataType(S, 519, SS_INT8);

35058


35059
  /* '<S123>/Generated S-Function5': DWORK6 */

35060
  ssSetDWorkName(S, 520, "DWORK520");

35061
  ssSetDWorkWidth(S, 520, 1);

35062
  ssSetDWorkDataType(S, 520, SS_INT8);

35063


35064
  /* '<S12>/LLACalibration2': DWORK1 */

35065
  ssSetDWorkName(S, 521, "DWORK521");

35066
  ssSetDWorkWidth(S, 521, 1);

35067
  ssSetDWorkDataType(S, 521, SS_UINT8);

35068


35069
  /* '<S12>/LLACalibration2': DWORK2 */

35070
  ssSetDWorkName(S, 522, "DWORK522");

35071
  ssSetDWorkWidth(S, 522, 1);

35072
  ssSetDWorkDataType(S, 522, SS_UINT8);

35073


35074
  /* '<S12>/LLACalibration2': DWORK3 */

35075
  ssSetDWorkName(S, 523, "DWORK523");

35076
  ssSetDWorkWidth(S, 523, 1);

35077
  ssSetDWorkDataType(S, 523, SS_UINT8);

35078


35079
  /* '<S32>/wowcmp1': DWORK1 */

35080
  ssSetDWorkName(S, 524, "DWORK524");

35081
  ssSetDWorkWidth(S, 524, 1);

35082
  ssSetDWorkDataType(S, 524, SS_UINT8);

35083


35084
  /* '<S32>/wowcmp1': DWORK2 */

35085
  ssSetDWorkName(S, 525, "DWORK525");

35086
  ssSetDWorkWidth(S, 525, 1);

35087
  ssSetDWorkDataType(S, 525, SS_UINT8);

35088


35089
  /* '<S32>/wowcmp1': DWORK3 */

35090
  ssSetDWorkName(S, 526, "DWORK526");

35091
  ssSetDWorkWidth(S, 526, 1);

35092
  ssSetDWorkDataType(S, 526, SS_UINT8);

35093


35094
  /* '<S322>/Taxi': DWORK1 */

35095
  ssSetDWorkName(S, 527, "DWORK527");

35096
  ssSetDWorkWidth(S, 527, 1);

35097
  ssSetDWorkDataType(S, 527, SS_UINT8);

35098


35099
  /* '<S322>/Taxi': DWORK2 */

35100
  ssSetDWorkName(S, 528, "DWORK528");

35101
  ssSetDWorkWidth(S, 528, 1);

35102
  ssSetDWorkDataType(S, 528, SS_UINT8);

35103


35104
  /* '<S322>/Taxi': DWORK3 */

35105
  ssSetDWorkName(S, 529, "DWORK529");

35106
  ssSetDWorkWidth(S, 529, 1);

35107
  ssSetDWorkDataType(S, 529, SS_UINT8);

35108


35109
  /* '<S322>/Taxi': DWORK4 */

35110
  ssSetDWorkName(S, 530, "DWORK530");

35111
  ssSetDWorkWidth(S, 530, 1);

35112
  ssSetDWorkDataType(S, 530, SS_UINT8);

35113


35114
  /* '<S322>/Taxi': DWORK5 */

35115
  ssSetDWorkName(S, 531, "DWORK531");

35116
  ssSetDWorkWidth(S, 531, 1);

35117
  ssSetDWorkDataType(S, 531, SS_UINT8);

35118


35119
  /* '<S316>/Landing2': DWORK1 */

35120
  ssSetDWorkName(S, 532, "DWORK532");

35121
  ssSetDWorkWidth(S, 532, 1);

35122
  ssSetDWorkDataType(S, 532, SS_UINT8);

35123


35124
  /* '<S316>/Landing2': DWORK2 */

35125
  ssSetDWorkName(S, 533, "DWORK533");

35126
  ssSetDWorkWidth(S, 533, 1);

35127
  ssSetDWorkDataType(S, 533, SS_UINT8);

35128


35129
  /* '<S316>/Landing2': DWORK3 */

35130
  ssSetDWorkName(S, 534, "DWORK534");

35131
  ssSetDWorkWidth(S, 534, 1);

35132
  ssSetDWorkDataType(S, 534, SS_UINT8);

35133


35134
  /* '<S316>/Landing2': DWORK4 */

35135
  ssSetDWorkName(S, 535, "DWORK535");

35136
  ssSetDWorkWidth(S, 535, 1);

35137
  ssSetDWorkDataType(S, 535, SS_UINT8);

35138


35139
  /* '<S316>/Landing2': DWORK5 */

35140
  ssSetDWorkName(S, 536, "DWORK536");

35141
  ssSetDWorkWidth(S, 536, 1);

35142
  ssSetDWorkDataType(S, 536, SS_UINT8);

35143


35144
  /* '<S316>/Landing2': DWORK6 */

35145
  ssSetDWorkName(S, 537, "DWORK537");

35146
  ssSetDWorkWidth(S, 537, 1);

35147
  ssSetDWorkDataType(S, 537, SS_UINT8);

35148


35149
  /* '<S316>/Landing2': DWORK7 */

35150
  ssSetDWorkName(S, 538, "DWORK538");

35151
  ssSetDWorkWidth(S, 538, 1);

35152
  ssSetDWorkDataType(S, 538, SS_UINT8);

35153


35154
  /* '<S316>/Landing2': DWORK8 */

35155
  ssSetDWorkName(S, 539, "DWORK539");

35156
  ssSetDWorkWidth(S, 539, 1);

35157
  ssSetDWorkDataType(S, 539, SS_UINT8);

35158


35159
  /* '<S316>/Landing2': DWORK9 */

35160
  ssSetDWorkName(S, 540, "DWORK540");

35161
  ssSetDWorkWidth(S, 540, 1);

35162
  ssSetDWorkDataType(S, 540, SS_UINT8);

35163


35164
  /* '<S316>/Landing2': DWORK10 */

35165
  ssSetDWorkName(S, 541, "DWORK541");

35166
  ssSetDWorkWidth(S, 541, 1);

35167
  ssSetDWorkDataType(S, 541, SS_UINT8);

35168


35169
  /* '<S316>/Landing2': DWORK11 */

35170
  ssSetDWorkName(S, 542, "DWORK542");

35171
  ssSetDWorkWidth(S, 542, 1);

35172
  ssSetDWorkDataType(S, 542, SS_UINT8);

35173


35174
  /* '<S316>/Landing2': DWORK12 */

35175
  ssSetDWorkName(S, 543, "DWORK543");

35176
  ssSetDWorkWidth(S, 543, 1);

35177
  ssSetDWorkDataType(S, 543, SS_UINT8);

35178


35179
  /* '<S316>/Landing2': DWORK13 */

35180
  ssSetDWorkName(S, 544, "DWORK544");

35181
  ssSetDWorkWidth(S, 544, 1);

35182
  ssSetDWorkDataType(S, 544, SS_UINT8);

35183


35184
  /* '<S316>/Landing2': DWORK14 */

35185
  ssSetDWorkName(S, 545, "DWORK545");

35186
  ssSetDWorkWidth(S, 545, 1);

35187
  ssSetDWorkDataType(S, 545, SS_UINT8);

35188


35189
  /* '<S316>/Landing2': DWORK15 */

35190
  ssSetDWorkName(S, 546, "DWORK546");

35191
  ssSetDWorkWidth(S, 546, 1);

35192
  ssSetDWorkDataType(S, 546, SS_UINT8);

35193


35194
  /* '<S313>/Approach2': DWORK1 */

35195
  ssSetDWorkName(S, 547, "DWORK547");

35196
  ssSetDWorkWidth(S, 547, 1);

35197
  ssSetDWorkDataType(S, 547, SS_UINT8);

35198


35199
  /* '<S313>/Approach2': DWORK2 */

35200
  ssSetDWorkName(S, 548, "DWORK548");

35201
  ssSetDWorkWidth(S, 548, 1);

35202
  ssSetDWorkDataType(S, 548, SS_UINT8);

35203


35204
  /* '<S313>/Approach2': DWORK3 */

35205
  ssSetDWorkName(S, 549, "DWORK549");

35206
  ssSetDWorkWidth(S, 549, 1);

35207
  ssSetDWorkDataType(S, 549, SS_UINT8);

35208


35209
  /* '<S313>/Approach2': DWORK4 */

35210
  ssSetDWorkName(S, 550, "DWORK550");

35211
  ssSetDWorkWidth(S, 550, 1);

35212
  ssSetDWorkDataType(S, 550, SS_UINT8);

35213


35214
  /* '<S313>/Approach2': DWORK5 */

35215
  ssSetDWorkName(S, 551, "DWORK551");

35216
  ssSetDWorkWidth(S, 551, 1);

35217
  ssSetDWorkDataType(S, 551, SS_UINT8);

35218


35219
  /* '<S313>/Approach2': DWORK6 */

35220
  ssSetDWorkName(S, 552, "DWORK552");

35221
  ssSetDWorkWidth(S, 552, 1);

35222
  ssSetDWorkDataType(S, 552, SS_UINT8);

35223


35224
  /* '<S313>/Approach2': DWORK7 */

35225
  ssSetDWorkName(S, 553, "DWORK553");

35226
  ssSetDWorkWidth(S, 553, 1);

35227
  ssSetDWorkDataType(S, 553, SS_UINT8);

35228


35229
  /* '<S313>/Approach2': DWORK8 */

35230
  ssSetDWorkName(S, 554, "DWORK554");

35231
  ssSetDWorkWidth(S, 554, 1);

35232
  ssSetDWorkDataType(S, 554, SS_UINT8);

35233


35234
  /* '<S313>/Approach2': DWORK9 */

35235
  ssSetDWorkName(S, 555, "DWORK555");

35236
  ssSetDWorkWidth(S, 555, 1);

35237
  ssSetDWorkDataType(S, 555, SS_UINT8);

35238


35239
  /* '<S313>/Approach2': DWORK10 */

35240
  ssSetDWorkName(S, 556, "DWORK556");

35241
  ssSetDWorkWidth(S, 556, 1);

35242
  ssSetDWorkDataType(S, 556, SS_UINT8);

35243


35244
  /* '<S315>/Cruise': DWORK1 */

35245
  ssSetDWorkName(S, 557, "DWORK557");

35246
  ssSetDWorkWidth(S, 557, 1);

35247
  ssSetDWorkDataType(S, 557, SS_UINT8);

35248


35249
  /* '<S315>/Cruise': DWORK2 */

35250
  ssSetDWorkName(S, 558, "DWORK558");

35251
  ssSetDWorkWidth(S, 558, 1);

35252
  ssSetDWorkDataType(S, 558, SS_UINT8);

35253


35254
  /* '<S315>/Cruise': DWORK3 */

35255
  ssSetDWorkName(S, 559, "DWORK559");

35256
  ssSetDWorkWidth(S, 559, 1);

35257
  ssSetDWorkDataType(S, 559, SS_UINT8);

35258


35259
  /* '<S315>/Cruise': DWORK4 */

35260
  ssSetDWorkName(S, 560, "DWORK560");

35261
  ssSetDWorkWidth(S, 560, 1);

35262
  ssSetDWorkDataType(S, 560, SS_UINT8);

35263


35264
  /* '<S315>/Cruise': DWORK5 */

35265
  ssSetDWorkName(S, 561, "DWORK561");

35266
  ssSetDWorkWidth(S, 561, 1);

35267
  ssSetDWorkDataType(S, 561, SS_UINT8);

35268


35269
  /* '<S315>/Cruise': DWORK6 */

35270
  ssSetDWorkName(S, 562, "DWORK562");

35271
  ssSetDWorkWidth(S, 562, 1);

35272
  ssSetDWorkDataType(S, 562, SS_UINT8);

35273


35274
  /* '<S315>/Cruise': DWORK7 */

35275
  ssSetDWorkName(S, 563, "DWORK563");

35276
  ssSetDWorkWidth(S, 563, 1);

35277
  ssSetDWorkDataType(S, 563, SS_UINT8);

35278


35279
  /* '<S315>/Cruise': DWORK8 */

35280
  ssSetDWorkName(S, 564, "DWORK564");

35281
  ssSetDWorkWidth(S, 564, 1);

35282
  ssSetDWorkDataType(S, 564, SS_UINT8);

35283


35284
  /* '<S315>/Cruise': DWORK9 */

35285
  ssSetDWorkName(S, 565, "DWORK565");

35286
  ssSetDWorkWidth(S, 565, 1);

35287
  ssSetDWorkDataType(S, 565, SS_UINT8);

35288


35289
  /* '<S315>/Cruise': DWORK10 */

35290
  ssSetDWorkName(S, 566, "DWORK566");

35291
  ssSetDWorkWidth(S, 566, 1);

35292
  ssSetDWorkDataType(S, 566, SS_UINT8);

35293


35294
  /* '<S314>/Climb2': DWORK1 */

35295
  ssSetDWorkName(S, 567, "DWORK567");

35296
  ssSetDWorkWidth(S, 567, 1);

35297
  ssSetDWorkDataType(S, 567, SS_UINT8);

35298


35299
  /* '<S314>/Climb2': DWORK2 */

35300
  ssSetDWorkName(S, 568, "DWORK568");

35301
  ssSetDWorkWidth(S, 568, 1);

35302
  ssSetDWorkDataType(S, 568, SS_UINT8);

35303


35304
  /* '<S314>/Climb2': DWORK3 */

35305
  ssSetDWorkName(S, 569, "DWORK569");

35306
  ssSetDWorkWidth(S, 569, 1);

35307
  ssSetDWorkDataType(S, 569, SS_UINT8);

35308


35309
  /* '<S314>/Climb2': DWORK4 */

35310
  ssSetDWorkName(S, 570, "DWORK570");

35311
  ssSetDWorkWidth(S, 570, 1);

35312
  ssSetDWorkDataType(S, 570, SS_UINT8);

35313


35314
  /* '<S314>/Climb2': DWORK5 */

35315
  ssSetDWorkName(S, 571, "DWORK571");

35316
  ssSetDWorkWidth(S, 571, 1);

35317
  ssSetDWorkDataType(S, 571, SS_UINT8);

35318


35319
  /* '<S314>/Climb2': DWORK6 */

35320
  ssSetDWorkName(S, 572, "DWORK572");

35321
  ssSetDWorkWidth(S, 572, 1);

35322
  ssSetDWorkDataType(S, 572, SS_UINT8);

35323


35324
  /* '<S314>/Climb2': DWORK7 */

35325
  ssSetDWorkName(S, 573, "DWORK573");

35326
  ssSetDWorkWidth(S, 573, 1);

35327
  ssSetDWorkDataType(S, 573, SS_UINT8);

35328


35329
  /* '<S314>/Climb2': DWORK8 */

35330
  ssSetDWorkName(S, 574, "DWORK574");

35331
  ssSetDWorkWidth(S, 574, 1);

35332
  ssSetDWorkDataType(S, 574, SS_UINT8);

35333


35334
  /* '<S314>/Climb2': DWORK9 */

35335
  ssSetDWorkName(S, 575, "DWORK575");

35336
  ssSetDWorkWidth(S, 575, 1);

35337
  ssSetDWorkDataType(S, 575, SS_UINT8);

35338


35339
  /* '<S321>/Takeoff': DWORK1 */

35340
  ssSetDWorkName(S, 576, "DWORK576");

35341
  ssSetDWorkWidth(S, 576, 1);

35342
  ssSetDWorkDataType(S, 576, SS_UINT8);

35343


35344
  /* '<S321>/Takeoff': DWORK2 */

35345
  ssSetDWorkName(S, 577, "DWORK577");

35346
  ssSetDWorkWidth(S, 577, 1);

35347
  ssSetDWorkDataType(S, 577, SS_UINT8);

35348


35349
  /* '<S321>/Takeoff': DWORK3 */

35350
  ssSetDWorkName(S, 578, "DWORK578");

35351
  ssSetDWorkWidth(S, 578, 1);

35352
  ssSetDWorkDataType(S, 578, SS_UINT8);

35353


35354
  /* '<S321>/Takeoff': DWORK4 */

35355
  ssSetDWorkName(S, 579, "DWORK579");

35356
  ssSetDWorkWidth(S, 579, 1);

35357
  ssSetDWorkDataType(S, 579, SS_UINT8);

35358


35359
  /* '<S319>/Pre_Takeoff': DWORK1 */

35360
  ssSetDWorkName(S, 580, "DWORK580");

35361
  ssSetDWorkWidth(S, 580, 1);

35362
  ssSetDWorkDataType(S, 580, SS_UINT8);

35363


35364
  /* '<S319>/Pre_Takeoff': DWORK2 */

35365
  ssSetDWorkName(S, 581, "DWORK581");

35366
  ssSetDWorkWidth(S, 581, 1);

35367
  ssSetDWorkDataType(S, 581, SS_UINT8);

35368


35369
  /* '<S319>/Pre_Takeoff': DWORK3 */

35370
  ssSetDWorkName(S, 582, "DWORK582");

35371
  ssSetDWorkWidth(S, 582, 1);

35372
  ssSetDWorkDataType(S, 582, SS_UINT8);

35373


35374
  /* '<S29>/LLACalibration2': DWORK1 */

35375
  ssSetDWorkName(S, 583, "DWORK583");

35376
  ssSetDWorkWidth(S, 583, 1);

35377
  ssSetDWorkDataType(S, 583, SS_UINT8);

35378


35379
  /* '<S29>/LLACalibration2': DWORK2 */

35380
  ssSetDWorkName(S, 584, "DWORK584");

35381
  ssSetDWorkWidth(S, 584, 1);

35382
  ssSetDWorkDataType(S, 584, SS_UINT8);

35383


35384
  /* '<S29>/LLACalibration2': DWORK3 */

35385
  ssSetDWorkName(S, 585, "DWORK585");

35386
  ssSetDWorkWidth(S, 585, 1);

35387
  ssSetDWorkDataType(S, 585, SS_UINT8);

35388


35389
  /* '<S24>/zeros2': DWORK1 */

35390
  ssSetDWorkName(S, 586, "DWORK586");

35391
  ssSetDWorkWidth(S, 586, 1);

35392
  ssSetDWorkDataType(S, 586, SS_UINT8);

35393


35394
  /* '<S24>/zeros2': DWORK2 */

35395
  ssSetDWorkName(S, 587, "DWORK587");

35396
  ssSetDWorkWidth(S, 587, 1);

35397
  ssSetDWorkDataType(S, 587, SS_UINT8);

35398


35399
  /* '<S299>/AirLineUpdateSucModel': DWORK1 */

35400
  ssSetDWorkName(S, 588, "DWORK588");

35401
  ssSetDWorkWidth(S, 588, 1);

35402
  ssSetDWorkDataType(S, 588, SS_UINT8);

35403


35404
  /* '<S299>/AirLineUpdateSucModel': DWORK2 */

35405
  ssSetDWorkName(S, 589, "DWORK589");

35406
  ssSetDWorkWidth(S, 589, 1);

35407
  ssSetDWorkDataType(S, 589, SS_UINT8);

35408


35409
  /* '<S299>/AirLineUpdateSucModel': DWORK3 */

35410
  ssSetDWorkName(S, 590, "DWORK590");

35411
  ssSetDWorkWidth(S, 590, 1);

35412
  ssSetDWorkDataType(S, 590, SS_UINT8);

35413


35414
  /* '<S21>/delayblock7': DWORK1 */

35415
  ssSetDWorkName(S, 591, "DWORK591");

35416
  ssSetDWorkWidth(S, 591, 1);

35417
  ssSetDWorkDataType(S, 591, SS_UINT8);

35418


35419
  /* '<S21>/delayblock7': DWORK2 */

35420
  ssSetDWorkName(S, 592, "DWORK592");

35421
  ssSetDWorkWidth(S, 592, 1);

35422
  ssSetDWorkDataType(S, 592, SS_UINT8);

35423


35424
  /* '<S21>/delayblock7': DWORK3 */

35425
  ssSetDWorkName(S, 593, "DWORK593");

35426
  ssSetDWorkWidth(S, 593, 1);

35427
  ssSetDWorkDataType(S, 593, SS_UINT8);

35428


35429
  /* '<S21>/delayblock6': DWORK1 */

35430
  ssSetDWorkName(S, 594, "DWORK594");

35431
  ssSetDWorkWidth(S, 594, 1);

35432
  ssSetDWorkDataType(S, 594, SS_UINT8);

35433


35434
  /* '<S21>/delayblock6': DWORK2 */

35435
  ssSetDWorkName(S, 595, "DWORK595");

35436
  ssSetDWorkWidth(S, 595, 1);

35437
  ssSetDWorkDataType(S, 595, SS_UINT8);

35438


35439
  /* '<S21>/delayblock6': DWORK3 */

35440
  ssSetDWorkName(S, 596, "DWORK596");

35441
  ssSetDWorkWidth(S, 596, 1);

35442
  ssSetDWorkDataType(S, 596, SS_UINT8);

35443


35444
  /* '<S21>/delayblock5': DWORK1 */

35445
  ssSetDWorkName(S, 597, "DWORK597");

35446
  ssSetDWorkWidth(S, 597, 1);

35447
  ssSetDWorkDataType(S, 597, SS_UINT8);

35448


35449
  /* '<S21>/delayblock5': DWORK2 */

35450
  ssSetDWorkName(S, 598, "DWORK598");

35451
  ssSetDWorkWidth(S, 598, 1);

35452
  ssSetDWorkDataType(S, 598, SS_UINT8);

35453


35454
  /* '<S21>/delayblock5': DWORK3 */

35455
  ssSetDWorkName(S, 599, "DWORK599");

35456
  ssSetDWorkWidth(S, 599, 1);

35457
  ssSetDWorkDataType(S, 599, SS_UINT8);

35458


35459
  /* '<S21>/delayblock4': DWORK1 */

35460
  ssSetDWorkName(S, 600, "DWORK600");

35461
  ssSetDWorkWidth(S, 600, 1);

35462
  ssSetDWorkDataType(S, 600, SS_UINT8);

35463


35464
  /* '<S21>/delayblock4': DWORK2 */

35465
  ssSetDWorkName(S, 601, "DWORK601");

35466
  ssSetDWorkWidth(S, 601, 1);

35467
  ssSetDWorkDataType(S, 601, SS_UINT8);

35468


35469
  /* '<S21>/delayblock4': DWORK3 */

35470
  ssSetDWorkName(S, 602, "DWORK602");

35471
  ssSetDWorkWidth(S, 602, 1);

35472
  ssSetDWorkDataType(S, 602, SS_UINT8);

35473


35474
  /* '<S21>/delayblock3': DWORK1 */

35475
  ssSetDWorkName(S, 603, "DWORK603");

35476
  ssSetDWorkWidth(S, 603, 1);

35477
  ssSetDWorkDataType(S, 603, SS_UINT8);

35478


35479
  /* '<S21>/delayblock3': DWORK2 */

35480
  ssSetDWorkName(S, 604, "DWORK604");

35481
  ssSetDWorkWidth(S, 604, 1);

35482
  ssSetDWorkDataType(S, 604, SS_UINT8);

35483


35484
  /* '<S21>/delayblock3': DWORK3 */

35485
  ssSetDWorkName(S, 605, "DWORK605");

35486
  ssSetDWorkWidth(S, 605, 1);

35487
  ssSetDWorkDataType(S, 605, SS_UINT8);

35488


35489
  /* '<S21>/delayblock2': DWORK1 */

35490
  ssSetDWorkName(S, 606, "DWORK606");

35491
  ssSetDWorkWidth(S, 606, 1);

35492
  ssSetDWorkDataType(S, 606, SS_UINT8);

35493


35494
  /* '<S21>/delayblock2': DWORK2 */

35495
  ssSetDWorkName(S, 607, "DWORK607");

35496
  ssSetDWorkWidth(S, 607, 1);

35497
  ssSetDWorkDataType(S, 607, SS_UINT8);

35498


35499
  /* '<S21>/delayblock2': DWORK3 */

35500
  ssSetDWorkName(S, 608, "DWORK608");

35501
  ssSetDWorkWidth(S, 608, 1);

35502
  ssSetDWorkDataType(S, 608, SS_UINT8);

35503


35504
  /* '<S21>/delayblock1': DWORK1 */

35505
  ssSetDWorkName(S, 609, "DWORK609");

35506
  ssSetDWorkWidth(S, 609, 1);

35507
  ssSetDWorkDataType(S, 609, SS_UINT8);

35508


35509
  /* '<S21>/delayblock1': DWORK2 */

35510
  ssSetDWorkName(S, 610, "DWORK610");

35511
  ssSetDWorkWidth(S, 610, 1);

35512
  ssSetDWorkDataType(S, 610, SS_UINT8);

35513


35514
  /* '<S21>/delayblock1': DWORK3 */

35515
  ssSetDWorkName(S, 611, "DWORK611");

35516
  ssSetDWorkWidth(S, 611, 1);

35517
  ssSetDWorkDataType(S, 611, SS_UINT8);

35518


35519
  /* '<S9>/Memory1': PreviousInput */

35520
  ssSetDWorkName(S, 612, "DWORK612");

35521
  ssSetDWorkWidth(S, 612, 1);

35522
  ssSetDWorkDataType(S, 612, SS_BOOLEAN);

35523


35524
  /* '<S299>/Enabled Subsystem': MODE */

35525
  ssSetDWorkName(S, 613, "DWORK613");

35526
  ssSetDWorkWidth(S, 613, 1);

35527
  ssSetDWorkDataType(S, 613, SS_BOOLEAN);

35528


35529
  /* '<S15>/Subsystem3': MODE */

35530
  ssSetDWorkName(S, 614, "DWORK614");

35531
  ssSetDWorkWidth(S, 614, 1);

35532
  ssSetDWorkDataType(S, 614, SS_BOOLEAN);

35533


35534
  /* '<S727>/Xcmpr_prev': PreviousInput */

35535
  ssSetDWorkName(S, 615, "DWORK615");

35536
  ssSetDWorkWidth(S, 615, 1);

35537


35538
  /* '<S717>/Scope1': PWORK */

35539
  ssSetDWorkName(S, 616, "DWORK616");

35540
  ssSetDWorkWidth(S, 616, 1);

35541
  ssSetDWorkDataType(S, 616, SS_POINTER);

35542


35543
  /* '<S717>/Scope2': PWORK */

35544
  ssSetDWorkName(S, 617, "DWORK617");

35545
  ssSetDWorkWidth(S, 617, 1);

35546
  ssSetDWorkDataType(S, 617, SS_POINTER);

35547


35548
  /* '<S717>/Scope5': PWORK */

35549
  ssSetDWorkName(S, 618, "DWORK618");

35550
  ssSetDWorkWidth(S, 618, 1);

35551
  ssSetDWorkDataType(S, 618, SS_POINTER);

35552


35553
  /* '<S720>/Scope1': PWORK */

35554
  ssSetDWorkName(S, 619, "DWORK619");

35555
  ssSetDWorkWidth(S, 619, 1);

35556
  ssSetDWorkDataType(S, 619, SS_POINTER);

35557


35558
  /* '<S720>/Scope10': PWORK */

35559
  ssSetDWorkName(S, 620, "DWORK620");

35560
  ssSetDWorkWidth(S, 620, 1);

35561
  ssSetDWorkDataType(S, 620, SS_POINTER);

35562


35563
  /* '<S720>/Scope11': PWORK */

35564
  ssSetDWorkName(S, 621, "DWORK621");

35565
  ssSetDWorkWidth(S, 621, 1);

35566
  ssSetDWorkDataType(S, 621, SS_POINTER);

35567


35568
  /* '<S720>/Scope7': PWORK */

35569
  ssSetDWorkName(S, 622, "DWORK622");

35570
  ssSetDWorkWidth(S, 622, 1);

35571
  ssSetDWorkDataType(S, 622, SS_POINTER);

35572


35573
  /* '<S621>/GenerateGroundFM': MODE */

35574
  ssSetDWorkName(S, 623, "DWORK623");

35575
  ssSetDWorkWidth(S, 623, 1);

35576
  ssSetDWorkDataType(S, 623, SS_BOOLEAN);

35577


35578
  /* '<S693>/Xcmpr_prev': PreviousInput */

35579
  ssSetDWorkName(S, 624, "DWORK624");

35580
  ssSetDWorkWidth(S, 624, 1);

35581


35582
  /* '<S686>/Scope1': PWORK */

35583
  ssSetDWorkName(S, 625, "DWORK625");

35584
  ssSetDWorkWidth(S, 625, 1);

35585
  ssSetDWorkDataType(S, 625, SS_POINTER);

35586


35587
  /* '<S686>/Scope5': PWORK */

35588
  ssSetDWorkName(S, 626, "DWORK626");

35589
  ssSetDWorkWidth(S, 626, 1);

35590
  ssSetDWorkDataType(S, 626, SS_POINTER);

35591


35592
  /* '<S688>/Scope8': PWORK */

35593
  ssSetDWorkName(S, 627, "DWORK627");

35594
  ssSetDWorkWidth(S, 627, 1);

35595
  ssSetDWorkDataType(S, 627, SS_POINTER);

35596


35597
  /* '<S620>/GenerateGroundFM': MODE */

35598
  ssSetDWorkName(S, 628, "DWORK628");

35599
  ssSetDWorkWidth(S, 628, 1);

35600
  ssSetDWorkDataType(S, 628, SS_BOOLEAN);

35601


35602
  /* '<S660>/Xcmpr_prev': PreviousInput */

35603
  ssSetDWorkName(S, 629, "DWORK629");

35604
  ssSetDWorkWidth(S, 629, 1);

35605


35606
  /* '<S618>/Scope1': PWORK */

35607
  ssSetDWorkName(S, 630, "DWORK630");

35608
  ssSetDWorkWidth(S, 630, 1);

35609
  ssSetDWorkDataType(S, 630, SS_POINTER);

35610


35611
  /* '<S618>/Scope2': PWORK */

35612
  ssSetDWorkName(S, 631, "DWORK631");

35613
  ssSetDWorkWidth(S, 631, 1);

35614
  ssSetDWorkDataType(S, 631, SS_POINTER);

35615


35616
  /* '<S651>/Scope1': PWORK */

35617
  ssSetDWorkName(S, 632, "DWORK632");

35618
  ssSetDWorkWidth(S, 632, 1);

35619
  ssSetDWorkDataType(S, 632, SS_POINTER);

35620


35621
  /* '<S651>/Scope2': PWORK */

35622
  ssSetDWorkName(S, 633, "DWORK633");

35623
  ssSetDWorkWidth(S, 633, 1);

35624
  ssSetDWorkDataType(S, 633, SS_POINTER);

35625


35626
  /* '<S651>/Scope3': PWORK */

35627
  ssSetDWorkName(S, 634, "DWORK634");

35628
  ssSetDWorkWidth(S, 634, 1);

35629
  ssSetDWorkDataType(S, 634, SS_POINTER);

35630


35631
  /* '<S651>/Scope5': PWORK */

35632
  ssSetDWorkName(S, 635, "DWORK635");

35633
  ssSetDWorkWidth(S, 635, 1);

35634
  ssSetDWorkDataType(S, 635, SS_POINTER);

35635


35636
  /* '<S654>/Scope1': PWORK */

35637
  ssSetDWorkName(S, 636, "DWORK636");

35638
  ssSetDWorkWidth(S, 636, 1);

35639
  ssSetDWorkDataType(S, 636, SS_POINTER);

35640


35641
  /* '<S654>/Scope10': PWORK */

35642
  ssSetDWorkName(S, 637, "DWORK637");

35643
  ssSetDWorkWidth(S, 637, 1);

35644
  ssSetDWorkDataType(S, 637, SS_POINTER);

35645


35646
  /* '<S654>/Scope11': PWORK */

35647
  ssSetDWorkName(S, 638, "DWORK638");

35648
  ssSetDWorkWidth(S, 638, 1);

35649
  ssSetDWorkDataType(S, 638, SS_POINTER);

35650


35651
  /* '<S654>/Scope2': PWORK */

35652
  ssSetDWorkName(S, 639, "DWORK639");

35653
  ssSetDWorkWidth(S, 639, 1);

35654
  ssSetDWorkDataType(S, 639, SS_POINTER);

35655


35656
  /* '<S618>/GenerateGroundFM': MODE */

35657
  ssSetDWorkName(S, 640, "DWORK640");

35658
  ssSetDWorkWidth(S, 640, 1);

35659
  ssSetDWorkDataType(S, 640, SS_BOOLEAN);

35660


35661
  /* Tunable Parameters */

35662
  ssSetNumSFcnParams(S, 0);

35663


35664
  /* Number of expected parameters */

35665
#if defined(MATLAB_MEX_FILE)

35666


35667
  if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {

35668


35669
#if defined(MDL_CHECK_PARAMETERS)

35670


35671
    mdlCheckParameters(S);

35672


35673
#endif                                 /* MDL_CHECK_PARAMETERS */

35674


35675
    if (ssGetErrorStatus(S) != (NULL) ) {

35676
      return;

35677
    }

35678
  } else {

35679
    return;                            /* Parameter mismatch will be reported by Simulink */

35680
  }

35681


35682
#endif                                 /* MATLAB_MEX_FILE */

35683


35684
  /* Options */

35685
  ssSetOptions(S, (SS_OPTION_RUNTIME_EXCEPTION_FREE_CODE ));

35686


35687
#if SS_SFCN_FOR_SIM

35688


35689
  {

35690
    ssSupportsMultipleExecInstances(S, false);

35691
    ssRegisterMsgForNotSupportingMultiExecInst(S,

35692
      "<diag_root><diag id=\"Simulink:blocks:ImplicitIterSS_TestPointNotSupportedInside\"><arguments><arg type=\"numeric\">4</arg><arg type=\"encoded\">QQBGAEMAUwBfAE0ATwBEAEUATAAxAC8AQQBGAEMAUwBfAE0ATwBEAEUATAAxAC8ARABvAHcAbgBsAGkAbgBrADEALwBCAHUAcwAgAFMAZQBsAGUAYwB0AG8AcgAAAA==</arg><arg type=\"encoded\">PABfAF8AaQBpAFMAUwBfAF8APgA8AC8AXwBfAGkAaQBTAFMAXwBfAD4AAAA=</arg><arg type=\"encoded\">PABfAF8AaQB0AGUAcgBCAGwAawBfAF8APgA8AC8AXwBfAGkAdABlAHIAQgBsAGsAXwBfAD4AAAA=</arg></arguments></diag>\n</diag_root>");

35693
    ssHasStateInsideForEachSS(S, false);

35694
  }

35695


35696
#endif

35697


35698
}

35699


35700
/* Function to initialize sample times. */

35701
static void mdlInitializeSampleTimes(SimStruct *S)

35702
{

35703
  /* task periods */

35704
  ssSetSampleTime(S, 0, 0.0);

35705
  ssSetSampleTime(S, 1, 0.005);

35706


35707
  /* task offsets */

35708
  ssSetOffsetTime(S, 0, 0.0);

35709
  ssSetOffsetTime(S, 1, 0.0);

35710
}

35711


35712
#if defined(MATLAB_MEX_FILE)

35713
# include "fixedpoint.c"

35714
# include "simulink.c"

35715
#else

35716
# undef S_FUNCTION_NAME

35717
# define S_FUNCTION_NAME               AFCS_MODEL1_sf

35718
# include "cg_sfun.h"

35719
#endif                                 /* defined(MATLAB_MEX_FILE) */

35720